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

Plane: use location_path_proportion()

Browse Source 
this should produce better glide slopes when the aircraft is off
course
This commit is contained in:
Andrew Tridgell 2015-01-01 15:17:45 +11:00
parent f38f86ab8c
commit ae96a48efc
7 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/ArduPlane.pde
View File

@ -565,6 +565,12 @@ static struct {
// time when we first pass min GPS speed on takeoff
uint32_t takeoff_speed_time_ms;
// distance to next waypoint
float wp_distance;
// proportion to next waypoint
float wp_proportion;
} auto_state = {
takeoff_complete : true,
land_complete : false,
@ -636,15 +642,6 @@ AP_Terrain terrain(ahrs, mission, rally);
APM_OBC obc(mission, barometer, gps, rcmap);
#endif
////////////////////////////////////////////////////////////////////////////////
// Waypoint distances
////////////////////////////////////////////////////////////////////////////////
// Distance between plane and next waypoint. Meters
static uint32_t wp_distance;
// Distance between previous and next waypoint. Meters
static uint32_t wp_totalDistance;
/*
meta data to support counting the number of circles in a loiter
*/

2
ArduPlane/GCS_Mavlink.pde
View File

@ -334,7 +334,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
nav_pitch_cd * 0.01,
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,
wp_distance,
auto_state.wp_distance,
altitude_error_cm * 0.01,
airspeed_error_cm,
nav_controller->crosstrack_error());

6
ArduPlane/Log.pde
View File

@ -303,7 +303,7 @@ struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint16_t yaw;
uint32_t wp_distance;
float wp_distance;
int16_t target_bearing_cd;
int16_t nav_bearing_cd;
int16_t altitude_error_cm;
@ -319,7 +319,7 @@ static void Log_Write_Nav_Tuning()
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_ms : hal.scheduler->millis(),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : wp_distance,
wp_distance : auto_state.wp_distance,
target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(),
altitude_error_cm : (int16_t)altitude_error_cm,
@ -582,7 +582,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Icccchhf", "TimeMS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "ICIccccfI", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
"NTUN", "ICfccccfI", "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "IffffBBf", "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" },
{ LOG_MODE_MSG, sizeof(log_Mode),

11
ArduPlane/altitude.pde
View File

@ -34,14 +34,14 @@ static void adjust_altitude_target()
set_target_altitude_location(next_WP_loc);
} else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
setup_landing_glide_slope();
} else if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
} else if (nav_controller->reached_loiter_target()) {
// once we reach a loiter target then lock to the final
// altitude target
set_target_altitude_location(next_WP_loc);
} else if (target_altitude.offset_cm != 0 &&
!location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
// control climb/descent rate
set_target_altitude_proportion(next_WP_loc, (float)(wp_distance-30) / (float)(wp_totalDistance-30));
set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
@ -59,8 +59,9 @@ static void setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude
wp_totalDistance = get_distance(current_loc, next_WP_loc);
wp_distance = wp_totalDistance;
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
/*
work out if we will gradually change altitude, or try to get to
@ -450,7 +451,7 @@ static float lookahead_adjustment(void)
distance = g.terrain_lookahead;
} else if (!nav_controller->reached_loiter_target()) {
bearing_cd = nav_controller->target_bearing_cd();
distance = constrain_float(wp_distance, 0, g.terrain_lookahead);
distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead);
} else {
// no lookahead when loitering
bearing_cd = 0;

9
ArduPlane/commands_logic.pde
View File

@ -444,7 +444,8 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
}
// see if the user has specified a maximum distance to waypoint
if (g.waypoint_max_radius > 0 && wp_distance > (uint16_t)g.waypoint_max_radius) {
if (g.waypoint_max_radius > 0 &&
auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
// this is needed to ensure completion of the waypoint
prev_WP_loc = current_loc;
@ -458,7 +459,7 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
acceptance_distance = cmd.p1;
}
if (wp_distance <= acceptance_distance) {
if (auto_state.wp_distance <= acceptance_distance) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP_loc));
@ -512,7 +513,7 @@ static bool verify_loiter_turns()
static bool verify_RTL()
{
update_loiter();
if (wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
nav_controller->reached_loiter_target()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
@ -600,7 +601,7 @@ static bool verify_change_alt()
static bool verify_within_distance()
{
if (wp_distance < max(condition_value,0)) {
if (auto_state.wp_distance < max(condition_value,0)) {
condition_value = 0;
return true;
}

5
ArduPlane/landing.pde
View File

@ -154,11 +154,10 @@ static void setup_landing_glide_slope(void)
target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
// calculate the proportion we are to the target
float land_distance = get_distance(current_loc, loc);
float land_total_distance = get_distance(prev_WP_loc, loc);
float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
// now setup the glide slope for landing
set_target_altitude_proportion(loc, land_distance / land_total_distance);
set_target_altitude_proportion(loc, 1.0f - land_proportion);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(loc, prev_WP_loc);

4
ArduPlane/navigation.pde
View File

@ -60,7 +60,9 @@ static void navigate()
// waypoint distance from plane
// ----------------------------
wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
// update total loiter angle
loiter_angle_update();