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

Copter: CH7 SaveWP fix corner case

Browse Source 
If the user is on the ground and flips CH7, do nothing instead of recording a bad takeoff altitude. Do another check to avoid a land right after a takeoff.
This commit is contained in:
Jason Short 2013-10-29 20:01:46 -07:00 committed by Randy Mackay
parent 8278767d0a
commit 2a82d1b0b0
1 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduCopter/control_modes.pde
View File

@ -190,23 +190,38 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
return; return;
} }
// we're on the ground
if((g.rc_3.control_in == 0) && (aux_switch_wp_index == 0)){
// nothing to do
return;
}
// initialise new waypoint to current location
Location new_wp;
if(aux_switch_wp_index == 0) { if(aux_switch_wp_index == 0) {
// this is our first WP, let's save WP 1 as a takeoff // this is our first WP, let's save WP 1 as a takeoff
// increment index to WP index of 1 (home is stored at 0) // increment index to WP index of 1 (home is stored at 0)
aux_switch_wp_index = 1; aux_switch_wp_index = 1;
Location temp = home;
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
temp.id = MAV_CMD_NAV_TAKEOFF; new_wp.id = MAV_CMD_NAV_TAKEOFF;
temp.alt = current_loc.alt; new_wp.options = 0;
new_wp.p1 = 0;
new_wp.lat = 0;
new_wp.lng = 0;
new_wp.alt = max(current_loc.alt,100);
// save command: // save command:
// we use the current altitude to be the target for takeoff. // we use the current altitude to be the target for takeoff.
// only altitude will matter to the AP mission script for takeoff. // only altitude will matter to the AP mission script for takeoff.
// If we are above the altitude, we will skip the command. // If we are above the altitude, we will skip the command.
set_cmd_with_index(temp, aux_switch_wp_index); set_cmd_with_index(new_wp, aux_switch_wp_index);
} }
// initialise new waypoint to current location
new_wp = current_loc;
// increment index // increment index
aux_switch_wp_index++; aux_switch_wp_index++;
@ -216,14 +231,14 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
if(g.rc_3.control_in > 0) { if(g.rc_3.control_in > 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
current_loc.id = MAV_CMD_NAV_WAYPOINT; new_wp.id = MAV_CMD_NAV_WAYPOINT;
}else{ }else{
// set our location ID to 21, MAV_CMD_NAV_LAND // set our location ID to 21, MAV_CMD_NAV_LAND
current_loc.id = MAV_CMD_NAV_LAND; new_wp.id = MAV_CMD_NAV_LAND;
} }
// save command // save command
set_cmd_with_index(current_loc, aux_switch_wp_index); set_cmd_with_index(new_wp, aux_switch_wp_index);
// log event // log event
Log_Write_Event(DATA_SAVEWP_ADD_WP); Log_Write_Event(DATA_SAVEWP_ADD_WP);