Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: inertial nav waypoint controller

Browse Source 
#define added to allow compile time selection of traditional or inav
based loiter and wp controllers
This commit is contained in:
Randy Mackay 2013-01-27 23:48:07 +09:00 committed by rmackay9
parent e0457f21de
commit fcf102b2cf
6 changed files with 164 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/ArduCopter.pde
View File

@ -700,6 +700,14 @@ static int32_t home_distance;
// distance between plane and next_WP in cm
// is not static because AP_Camera uses it
uint32_t wp_distance;
// wpinav variables
Vector2f wpinav_origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector2f wpinav_destination; // target destination in cm from home (equivalent to next_WP)
Vector2f wpinav_target; // the intermediate target location in cm from home
Vector2f wpinav_pos_delta; // position difference between origin and destination
float wpinav_track_length; // distance in cm between origin and destination
float wpinav_track_desired; // the desired distance along the track in cm
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors
@ -1777,9 +1785,9 @@ void update_simple_mode(void)
g.rc_2.control_in = _pitch;
}
// update_super_simple_beading - adjusts simple bearing based on location
// update_super_simple_bearing - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void update_super_simple_beading()
void update_super_simple_bearing()
{
// are we in SIMPLE mode?
if(ap.simple_mode && g.super_simple) {

17
ArduCopter/commands.pde
View File

@ -161,8 +161,25 @@ static void set_next_WP(struct Location *wp)
// to check if we have missed the WP
// ---------------------------------
original_wp_bearing = wp_bearing;
#if NAV_WP_ACTIVE == NAV_WP_INAV
// set inertial navigation controller's target incase it's called
wpinav_set_origin_and_destination(prev_WP, next_WP);
#endif
}
// set_next_WP_latlon - update just lat and lon targets for nav controllers
static void set_next_WP_latlon(uint32_t lat, uint32_t lon)
{
// Load the next_WP slot
next_WP.lat = lat;
next_WP.lng = lon;
#if NAV_WP_ACTIVE == NAV_WP_INAV
// set inertial navigation controller's target incase it's called
wpinav_set_destination(next_WP);
#endif
}
// run this at setup on the ground
// -------------------------------

40
ArduCopter/commands_logic.pde
View File

@ -226,7 +226,7 @@ static void do_RTL(void)
set_throttle_mode(RTL_THR);
// set navigation mode
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
// initial climb starts at current location
set_next_WP(&current_loc);
@ -242,7 +242,7 @@ static void do_RTL(void)
// do_takeoff - initiate takeoff navigation command
static void do_takeoff()
{
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
// Start with current location
Location temp = current_loc;
@ -263,7 +263,7 @@ static void do_takeoff()
// do_nav_wp - initiate move to next waypoint
static void do_nav_wp()
{
set_nav_mode(NAV_WP);
set_nav_mode(NAV_WP_ACTIVE);
set_next_WP(&command_nav_queue);
@ -292,7 +292,7 @@ static void do_land()
{
// hold at our current location
set_next_WP(&current_loc);
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
// hold current heading
set_yaw_mode(YAW_HOLD);
@ -302,15 +302,15 @@ static void do_land()
static void do_loiter_unlimited()
{
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
//cliSerial->println("dloi ");
if(command_nav_queue.lat == 0) {
set_next_WP(&current_loc);
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
}else{
set_next_WP(&command_nav_queue);
set_nav_mode(NAV_WP);
set_nav_mode(NAV_WP_ACTIVE);
}
}
@ -344,11 +344,12 @@ static void do_loiter_turns()
static void do_loiter_time()
{
if(command_nav_queue.lat == 0) {
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
loiter_time = millis();
set_next_WP(&current_loc);
}else{
set_nav_mode(NAV_WP);
set_nav_mode(NAV_WP_ACTIVE);
set_next_WP(&command_nav_queue);
}
@ -403,12 +404,10 @@ static bool verify_nav_wp()
// we have reached our goal
// loiter at the WP
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//cliSerial->println("vlt done");
}
}
@ -424,9 +423,9 @@ static bool verify_nav_wp()
static bool verify_loiter_unlimited()
{
if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) {
if(nav_mode == NAV_WP_ACTIVE && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) {
// switch to position hold
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
}
return false;
}
@ -434,16 +433,16 @@ static bool verify_loiter_unlimited()
// verify_loiter_time - check if we have loitered long enough
static bool verify_loiter_time()
{
if(nav_mode == NAV_LOITER) {
if(nav_mode == NAV_LOITER_ACTIVE) {
if ((millis() - loiter_time) > loiter_time_max) {
return true;
}
}
if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) {
if(nav_mode == NAV_WP_ACTIVE && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) {
// reset our loiter time
loiter_time = millis();
// switch to position hold
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
}
return false;
}
@ -483,7 +482,7 @@ static bool verify_RTL()
set_new_altitude(get_RTL_alt());
// set navigation mode
set_nav_mode(NAV_WP);
set_nav_mode(NAV_WP_ACTIVE);
// set yaw mode
set_yaw_mode(RTL_YAW);
@ -497,7 +496,7 @@ static bool verify_RTL()
// if we've reached home initiate loiter
if (wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0) || check_missed_wp()) {
rtl_state = RTL_STATE_LOITERING_AT_HOME;
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
// set loiter timer
rtl_loiter_start_time = millis();
@ -515,8 +514,7 @@ static bool verify_RTL()
// land
do_land();
// override landing location (do_land defaults to current location)
next_WP.lat = home.lat;
next_WP.lng = home.lng;
set_next_WP_latlon(home.lat, home.lng);
// update RTL state
rtl_state = RTL_STATE_LAND;
}else{

13
ArduCopter/config.h
View File

@ -607,6 +607,15 @@
#define EARTH_FRAME 0
#define BODY_FRAME 1
// active NAV controller
#ifndef NAV_WP_ACTIVE
# define NAV_WP_ACTIVE NAV_WP
#endif
// active LOITER controller
#ifndef NAV_LOITER_ACTIVE
# define NAV_LOITER_ACTIVE NAV_LOITER
#endif
// Flight mode roll, pitch, yaw, throttle and navigation definitions
// Acro Mode
@ -704,7 +713,7 @@
#endif
#ifndef LOITER_NAV
# define LOITER_NAV NAV_LOITER
# define LOITER_NAV NAV_LOITER_ACTIVE
#endif
// POSITION Mode
@ -721,7 +730,7 @@
#endif
#ifndef POSITION_NAV
# define POSITION_NAV NAV_LOITER
# define POSITION_NAV NAV_LOITER_ACTIVE
#endif

1
ArduCopter/defines.h
View File

@ -200,6 +200,7 @@
#define NAV_LOITER 2
#define NAV_WP 3
#define NAV_LOITER_INAV 4
#define NAV_WP_INAV 5
// Yaw override behaviours - used for setting yaw_override_behaviour
#define YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT 0 // auto pilot takes back yaw control at next waypoint

115
ArduCopter/navigation.pde
View File

@ -168,7 +168,7 @@ static void calc_distance_and_bearing()
home_bearing = get_bearing_cd(&current_loc, &home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_beading();
update_super_simple_bearing();
// bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
@ -203,7 +203,7 @@ static void run_autopilot()
case GUIDED:
// switch to loiter once we've reached the target location and altitude
if(verify_nav_wp()) {
set_nav_mode(NAV_LOITER);
set_nav_mode(NAV_LOITER_ACTIVE);
}
case RTL:
verify_RTL();
@ -249,6 +249,10 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_LOITER_INAV:
loiter_set_target(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff());
nav_initialised = set_roll_pitch_mode(ROLL_PITCH_LOITER_INAV);
break;
case NAV_WP_INAV:
nav_initialised = true;
break;
}
@ -296,8 +300,10 @@ static void update_nav_mode()
if (circle_angle > 6.28318531f)
circle_angle -= 6.28318531f;
next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp);
next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle));
// update target location
set_next_WP_latlon(
circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle)),
circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp));
// use error as the desired rate towards the target
// nav_lon, nav_lat is calculated
@ -326,8 +332,7 @@ static void update_nav_mode()
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
ap.loiter_override = false;
// reset LOITER to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
set_next_WP_latlon(current_loc.lat, current_loc.lng);
}
// We bring copy over our Iterms for wind control, but we don't navigate
nav_lon = g.pid_loiter_rate_lon.get_integrator();
@ -354,6 +359,10 @@ static void update_nav_mode()
case NAV_LOITER_INAV:
get_loiter_pos_lat_lon(loiter_lat_from_home_cm, loiter_lon_from_home_cm, 0.1f);
break;
case NAV_WP_INAV:
get_wpinav_pos(0.1f);
break;
}
/*
@ -759,7 +768,7 @@ get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt)
#define MAX_LOITER_POS_VELOCITY 750 // should be 1.5 ~ 2.0 times the pilot input's max velocity
#define MAX_LOITER_POS_ACCEL 250
static void
get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon, float dt)
get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt)
{
static float dist_error_lat;
int32_t desired_vel_lat;
@ -778,8 +787,8 @@ get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon, float dt)
// 100hz sample rate, 2hz filter, alpha = 0.11164f
// 20hz sample rate, 2hz filter, alpha = 0.38587f
// 10hz sample rate, 2hz filter, alpha = 0.55686f
dist_error_lat = dist_error_lat + 0.55686f * ((target_lat - inertial_nav.get_latitude_diff()) - dist_error_lat);
dist_error_lon = dist_error_lon + 0.55686f * ((target_lon - inertial_nav.get_longitude_diff()) - dist_error_lon);
dist_error_lat = dist_error_lat + 0.55686f * ((target_lat_from_home - inertial_nav.get_latitude_diff()) - dist_error_lat);
dist_error_lon = dist_error_lon + 0.55686f * ((target_lon_from_home - inertial_nav.get_longitude_diff()) - dist_error_lon);
linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
@ -842,3 +851,91 @@ loiter_set_target(float lat_from_home_cm, float lon_from_home_cm)
next_WP.lat = home.lat + loiter_lat_from_home_cm;
next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp;
}
//////////////////////////////////////////////////////////
// waypoint inertial navigation controller
//////////////////////////////////////////////////////////
// Waypoint navigation is accomplished by moving the target location up to a maximum of 10m from the current location
// get_wpinav_pos - wpinav position controller with desired position held in wpinav_destination
static void
get_wpinav_pos(float dt)
{
// reuse loiter position controller
get_loiter_pos_lat_lon(wpinav_target.x, wpinav_target.y, dt);
}
// wpinav_set_destination - set destination using lat/lon coordinates
void wpinav_set_destination(Location& destination)
{
wpinav_origin.x = inertial_nav.get_latitude_diff();
wpinav_origin.y = inertial_nav.get_longitude_diff();
wpinav_destination.x = (destination.lat-home.lat) * LATLON_TO_CM;
wpinav_destination.y = (destination.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
wpinav_pos_delta = wpinav_destination - wpinav_origin;
wpinav_track_length = wpinav_pos_delta.length();
wpinav_track_desired = 0;
}
// wpinav_set_origin_and_destination - set origin and destination using lat/lon coordinates
void wpinav_set_origin_and_destination(Location& origin, Location& destination)
{
wpinav_origin.x = (origin.lat-home.lat) * LATLON_TO_CM;
wpinav_origin.y = (origin.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
wpinav_destination.x = (destination.lat-home.lat) * LATLON_TO_CM;
wpinav_destination.y = (destination.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
wpinav_pos_delta = wpinav_destination - wpinav_origin;
wpinav_track_length = wpinav_pos_delta.length();
// reset the desired distance along the track to the closest point's distance
Vector2f curr(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff());
float line_a = wpinav_pos_delta.y;
float line_b = -wpinav_pos_delta.x;
float line_c = wpinav_pos_delta.x * wpinav_origin.y - wpinav_pos_delta.y * wpinav_origin.x;
float line_m = line_a / line_b;
line_m = 1/line_m;
line_a = line_m;
line_b = -1;
line_c = curr.y - line_m * curr.x;
// calculate the distance to the closest point along the track and it's distance from the origin
wpinav_track_desired = abs(line_a*wpinav_origin.x + line_b*wpinav_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b);
}
#define WPINAV_MAX_POS_ERROR 100.0f // maximum distance (in cm) that the desired track can stray from our current location.
void
wpinav_advance_track_desired(float velocity_cms, float dt)
{
// get current location
Vector2f curr(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff());
float line_a = wpinav_pos_delta.y;
float line_b = -wpinav_pos_delta.x;
float line_c = wpinav_pos_delta.x * wpinav_origin.y - wpinav_pos_delta.y * wpinav_origin.x;
float line_m = line_a / line_b;
float cross_track_dist = abs(line_a * curr.x + line_b * curr.y + line_c ) / wpinav_track_length;
line_m = 1/line_m;
line_a = line_m;
line_b = -1;
line_c = curr.y - line_m * curr.x;
// calculate the distance to the closest point along the track and it's distance from the origin
float track_covered = abs(line_a*wpinav_origin.x + line_b*wpinav_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b);
// maximum distance along the track that we will allow (stops target point from getting too far from the current position)
float track_desired_max = track_covered + safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR-cross_track_dist*cross_track_dist);
// advance the current target
wpinav_track_desired += velocity_cms * dt;
// constrain the target from moving too far
if( wpinav_track_desired > track_desired_max ) {
wpinav_track_desired = track_desired_max;
}
// recalculate the desired position
float track_length_pct = wpinav_track_desired/wpinav_track_length;
wpinav_target.x = wpinav_origin.x + wpinav_pos_delta.x * track_length_pct;
wpinav_target.y = wpinav_origin.y + wpinav_pos_delta.y * track_length_pct;
}