aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add zero slope checks to wpinav

Browse Source 
also added RTL fix to set wpinav target explicitly
This commit is contained in:
Randy Mackay 2013-01-29 00:01:40 +09:00 committed by rmackay9
parent b74da54c98
commit 37d3e1d7e4
3 changed files with 65 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/commands.pde
View File

@ -161,11 +161,6 @@ 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
@ -174,11 +169,6 @@ 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

16
ArduCopter/commands_logic.pde
View File

@ -228,9 +228,6 @@ static void do_RTL(void)
// set navigation mode
set_nav_mode(NAV_LOITER_ACTIVE);
// initial climb starts at current location
set_next_WP(&current_loc);
// override altitude to RTL altitude
set_new_altitude(get_RTL_alt());
}
@ -475,15 +472,20 @@ static bool verify_RTL()
case RTL_STATE_INITIAL_CLIMB:
// rely on verify_altitude function to update alt_change_flag when we've reached the target
if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) {
// Set navigation target to home
set_next_WP(&home);
// override target altitude to RTL altitude
set_new_altitude(get_RTL_alt());
// set navigation mode
set_nav_mode(NAV_WP_ACTIVE);
#if NAV_WP_ACTIVE == NAV_WP
// Set navigation target to home
set_next_WP(&home);
#else
// Set inav navigation target to home
wpinav_set_destination(home);
#endif
// set yaw mode
set_yaw_mode(RTL_YAW);
@ -514,6 +516,7 @@ static bool verify_RTL()
// land
do_land();
// override landing location (do_land defaults to current location)
// To-do: ensure this location override is being sent to inav loiter controller
set_next_WP_latlon(home.lat, home.lng);
// update RTL state
rtl_state = RTL_STATE_LAND;
@ -534,6 +537,7 @@ static bool verify_RTL()
// switch to regular loiter mode
set_mode(LOITER);
// override location and altitude
// To-Do: ensure this target location is being sent to loiter controller
set_next_WP(&home);
// override altitude to RTL altitude
set_new_altitude(g.rtl_alt_final);

97
ArduCopter/navigation.pde
View File

@ -248,8 +248,7 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_LOITER:
// set target 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);
nav_initialised = true;
break;
@ -259,6 +258,7 @@ 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());
// To-Do: below allows user to move around set point but do we want to allow this when in Auto flight mode?
nav_initialised = set_roll_pitch_mode(ROLL_PITCH_LOITER_INAV);
break;
@ -311,6 +311,7 @@ static void update_nav_mode()
circle_angle -= 6.28318531f;
// update target location
// To-Do: ensure this target is updated for inertial navigation controller
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));
@ -860,11 +861,13 @@ loiter_set_pos_from_velocity(int16_t vel_forward_cms, int16_t vel_right_cms, flo
loiter_lon_from_home_cm += vel_lon * dt;
// update next_WP location for reporting purposes
next_WP.lat = home.lat + loiter_lat_from_home_cm;
next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp;
set_next_WP_latlon(
home.lat + loiter_lat_from_home_cm / LATLON_TO_CM,
home.lng + loiter_lat_from_home_cm / LATLON_TO_CM * scaleLongUp);
}
// loiter_set_target - set loiter's target position from home in cm
// To-Do: change this function to accept a target in lat/lon format (and remove setting of next_WP?)
static void
loiter_set_target(float lat_from_home_cm, float lon_from_home_cm)
{
@ -872,8 +875,9 @@ loiter_set_target(float lat_from_home_cm, float lon_from_home_cm)
loiter_lon_from_home_cm = lon_from_home_cm;
// update next_WP location for reporting purposes
next_WP.lat = home.lat + loiter_lat_from_home_cm;
next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp;
set_next_WP_latlon(
home.lat + loiter_lat_from_home_cm / LATLON_TO_CM,
home.lng + loiter_lat_from_home_cm / LATLON_TO_CM * scaleLongUp);
}
//////////////////////////////////////////////////////////
@ -885,24 +889,18 @@ loiter_set_target(float lat_from_home_cm, float lon_from_home_cm)
static void
get_wpinav_pos(float dt)
{
// reuse loiter position controller
// re-use 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)
void wpinav_set_destination(const 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(current_loc, destination);
}
// wpinav_set_origin_and_destination - set origin and destination using lat/lon coordinates
void wpinav_set_origin_and_destination(Location& origin, Location& destination)
void wpinav_set_origin_and_destination(const Location& origin, const Location& destination)
{
wpinav_origin.x = (origin.lat-home.lat) * LATLON_TO_CM;
wpinav_origin.y = (origin.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
@ -910,45 +908,60 @@ void wpinav_set_origin_and_destination(Location& origin, Location& destination)
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;
// 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);
// set next_WP, prev_WP for reporting purposes
// To-Do: move calcs below to a function
set_next_WP_latlon(
home.lat + wpinav_destination.x / LATLON_TO_CM,
home.lng + wpinav_destination.y / LATLON_TO_CM * scaleLongUp);
}
#define WPINAV_MAX_POS_ERROR 100.0f // maximum distance (in cm) that the desired track can stray from our current location.
#define WPINAV_MAX_POS_ERROR 2000.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)
{
float cross_track_dist;
float track_covered;
float track_desired_max;
float line_a, line_b, line_c, line_m;
// 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;
// check for zero length segment
if( wpinav_pos_delta.x == 0 && wpinav_pos_delta.y == 0) {
wpinav_target = wpinav_destination;
return;
}
line_m = 1/line_m;
line_a = line_m;
line_b = -1;
line_c = curr.y - line_m * curr.x;
if( wpinav_pos_delta.x == 0 ) {
// x is zero
cross_track_dist = fabs(curr.x - wpinav_destination.x);
track_covered = fabs(curr.y - wpinav_origin.y);
}else if(wpinav_pos_delta.y == 0) {
// y is zero
cross_track_dist = fabs(curr.y - wpinav_destination.y);
track_covered = fabs(curr.x - wpinav_origin.x);
}else{
// both x and y non zero
line_a = wpinav_pos_delta.y;
line_b = -wpinav_pos_delta.x;
line_c = wpinav_pos_delta.x * wpinav_origin.y - wpinav_pos_delta.y * wpinav_origin.x;
line_m = line_a / line_b;
cross_track_dist = abs(line_a * curr.x + line_b * curr.y + line_c ) / wpinav_track_length;
// 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);
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
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);
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;