Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: moved auto mode variables to auto_state

This commit is contained in:
Andrew Tridgell 2014-04-23 21:57:51 +10:00
parent dba8fac515
commit 0306dbf5f1
3 changed files with 43 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
ArduPlane/ArduPlane.pde
View File

@ -514,16 +514,24 @@ static struct {
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// flight mode specific // flight mode specific
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process. static struct {
static bool takeoff_complete = true; // Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
// Flag to indicate if we have landed. bool takeoff_complete;
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
static bool land_complete;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
static int32_t takeoff_altitude_cm;
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree // Flag to indicate if we have landed.
static int16_t takeoff_pitch_cd; // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
bool land_complete;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
int32_t takeoff_altitude_cm;
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
int16_t takeoff_pitch_cd;
} auto_state = {
takeoff_complete : true,
land_complete : false,
takeoff_altitude_cm : 0,
takeoff_pitch_cd : 0
};
// true if we are in an auto-throttle mode, which means // true if we are in an auto-throttle mode, which means
// we need to run the speed/height controller // we need to run the speed/height controller
@ -1110,11 +1118,11 @@ static void handle_auto_mode(void)
if (airspeed.use()) { if (airspeed.use()) {
calc_nav_pitch(); calc_nav_pitch();
if (nav_pitch_cd < takeoff_pitch_cd) if (nav_pitch_cd < auto_state.takeoff_pitch_cd)
nav_pitch_cd = takeoff_pitch_cd; nav_pitch_cd = auto_state.takeoff_pitch_cd;
} else { } else {
nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd; nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd);
} }
// max throttle for takeoff // max throttle for takeoff
@ -1124,7 +1132,7 @@ static void handle_auto_mode(void)
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
calc_nav_roll(); calc_nav_roll();
if (land_complete) { if (auto_state.land_complete) {
// during final approach constrain roll to the range // during final approach constrain roll to the range
// allowed for level flight // allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
@ -1141,7 +1149,7 @@ static void handle_auto_mode(void)
} }
calc_throttle(); calc_throttle();
if (land_complete) { if (auto_state.land_complete) {
// we are in the final stage of a landing - force // we are in the final stage of a landing - force
// zero throttle // zero throttle
channel_throttle->servo_out = 0; channel_throttle->servo_out = 0;
@ -1152,7 +1160,7 @@ static void handle_auto_mode(void)
// we are doing normal AUTO flight, the special cases // we are doing normal AUTO flight, the special cases
// are for takeoff and landing // are for takeoff and landing
steer_state.hold_course_cd = -1; steer_state.hold_course_cd = -1;
land_complete = false; auto_state.land_complete = false;
calc_nav_roll(); calc_nav_roll();
calc_nav_pitch(); calc_nav_pitch();
calc_throttle(); calc_throttle();
@ -1388,9 +1396,10 @@ static void update_alt()
// Update the speed & height controller states // Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) { if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) { if (control_mode==AUTO) {
if (takeoff_complete == false) { if (auto_state.takeoff_complete == false) {
update_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); update_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && land_complete == true) { } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
auto_state.land_complete == true) {
update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
@ -1404,7 +1413,7 @@ static void update_alt()
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
target_airspeed_cm, target_airspeed_cm,
flight_stage, flight_stage,
takeoff_pitch_cd, auto_state.takeoff_pitch_cd,
throttle_nudge, throttle_nudge,
relative_altitude()); relative_altitude());
if (should_log(MASK_LOG_TECS)) { if (should_log(MASK_LOG_TECS)) {

4
ArduPlane/Attitude.pde
View File

@ -607,7 +607,9 @@ static bool suppress_throttle(void)
return false; return false;
} }
if (control_mode==AUTO && takeoff_complete == false && auto_takeoff_check()) { if (control_mode==AUTO &&
auto_state.takeoff_complete == false &&
auto_takeoff_check()) {
// we're in auto takeoff // we're in auto takeoff
throttle_suppressed = false; throttle_suppressed = false;
if (steer_state.hold_course_cd != -1) { if (steer_state.hold_course_cd != -1) {

24
ArduPlane/commands_logic.pde
View File

@ -30,15 +30,15 @@ start_command(const AP_Mission::Mission_Command& cmd)
// special handling for nav vs non-nav commands // special handling for nav vs non-nav commands
if (AP_Mission::is_nav_cmd(cmd)) { if (AP_Mission::is_nav_cmd(cmd)) {
// set land_complete to false to stop us zeroing the throttle // set land_complete to false to stop us zeroing the throttle
land_complete = false; auto_state.land_complete = false;
// set takeoff_complete to true so we don't add extra evevator // set takeoff_complete to true so we don't add extra evevator
// except in a takeoff // except in a takeoff
takeoff_complete = true; auto_state.takeoff_complete = true;
gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),cmd.id); gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),cmd.id);
}else{ } else {
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id); gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
} }
@ -259,11 +259,11 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{ {
set_next_WP(cmd.content.location); set_next_WP(cmd.content.location);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch_cd = (int)cmd.p1 * 100; auto_state.takeoff_pitch_cd = (int)cmd.p1 * 100;
takeoff_altitude_cm = next_WP_loc.alt; auto_state.takeoff_altitude_cm = next_WP_loc.alt;
next_WP_loc.lat = home.lat + 10; next_WP_loc.lat = home.lat + 10;
next_WP_loc.lng = home.lng + 10; next_WP_loc.lng = home.lng + 10;
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable // Flag also used to override "on the ground" throttle disable
} }
@ -329,9 +329,9 @@ static bool verify_takeoff()
} }
// see if we have reached takeoff altitude // see if we have reached takeoff altitude
if (adjusted_altitude_cm() > takeoff_altitude_cm) { if (adjusted_altitude_cm() > auto_state.takeoff_altitude_cm) {
steer_state.hold_course_cd = -1; steer_state.hold_course_cd = -1;
takeoff_complete = true; auto_state.takeoff_complete = true;
next_WP_loc = prev_WP_loc = current_loc; next_WP_loc = prev_WP_loc = current_loc;
#if GEOFENCE_ENABLED == ENABLED #if GEOFENCE_ENABLED == ENABLED
@ -362,7 +362,7 @@ static bool verify_land()
if ((wp_distance <= (g.land_flare_sec * gps.ground_speed())) if ((wp_distance <= (g.land_flare_sec * gps.ground_speed()))
|| (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) { || (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) {
land_complete = true; auto_state.land_complete = true;
if (steer_state.hold_course_cd == -1) { if (steer_state.hold_course_cd == -1) {
// we have just reached the threshold of to flare for landing. // we have just reached the threshold of to flare for landing.