mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
1cd3c21774
Changes mission structure so that conditional and immediate commands are located between associated waypoints instead of after the second waypoint.
532 lines
12 KiB
Plaintext
532 lines
12 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/********************************************************************************/
|
|
// Command Event Handlers
|
|
/********************************************************************************/
|
|
static void
|
|
handle_process_nav_cmd()
|
|
{
|
|
// reset navigation integrators
|
|
// -------------------------
|
|
reset_I();
|
|
|
|
switch(next_nav_command.id){
|
|
|
|
case MAV_CMD_NAV_TAKEOFF:
|
|
do_takeoff();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
|
|
do_nav_wp();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LAND: // LAND to Waypoint
|
|
do_land();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
|
|
do_loiter_unlimited();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
|
|
do_loiter_turns();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_TIME:
|
|
do_loiter_time();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
|
do_RTL();
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void
|
|
handle_process_condition_command()
|
|
{
|
|
switch(next_nonnav_command.id){
|
|
|
|
case MAV_CMD_CONDITION_DELAY:
|
|
do_wait_delay();
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_DISTANCE:
|
|
do_within_distance();
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_CHANGE_ALT:
|
|
do_change_alt();
|
|
break;
|
|
|
|
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
|
|
|
|
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
|
landing_pitch = next_nav_command.lng * 100;
|
|
g.airspeed_cruise = next_nav_command.alt * 100;
|
|
g.throttle_cruise = next_nav_command.lat;
|
|
landing_distance = next_nav_command.p1;
|
|
|
|
SendDebug_P("MSG: throttle_cruise = ");
|
|
SendDebugln(g.throttle_cruise,DEC);
|
|
break;
|
|
*/
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void handle_process_do_command()
|
|
{
|
|
switch(next_nonnav_command.id){
|
|
|
|
case MAV_CMD_DO_JUMP:
|
|
do_jump();
|
|
break;
|
|
|
|
case MAV_CMD_DO_CHANGE_SPEED:
|
|
do_change_speed();
|
|
break;
|
|
|
|
case MAV_CMD_DO_SET_HOME:
|
|
do_set_home();
|
|
break;
|
|
|
|
case MAV_CMD_DO_SET_SERVO:
|
|
do_set_servo();
|
|
break;
|
|
|
|
case MAV_CMD_DO_SET_RELAY:
|
|
do_set_relay();
|
|
break;
|
|
|
|
case MAV_CMD_DO_REPEAT_SERVO:
|
|
do_repeat_servo();
|
|
break;
|
|
|
|
case MAV_CMD_DO_REPEAT_RELAY:
|
|
do_repeat_relay();
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void handle_no_commands()
|
|
{
|
|
next_nav_command = home;
|
|
next_nav_command.alt = read_alt_to_hold();
|
|
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Verify command Handlers
|
|
/********************************************************************************/
|
|
|
|
static bool verify_nav_command() // Returns true if command complete
|
|
{
|
|
switch(nav_command_ID) {
|
|
|
|
case MAV_CMD_NAV_TAKEOFF:
|
|
return verify_takeoff();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LAND:
|
|
return verify_land();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_WAYPOINT:
|
|
return verify_nav_wp();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_UNLIM:
|
|
return verify_loiter_unlim();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_TURNS:
|
|
return verify_loiter_turns();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_LOITER_TIME:
|
|
return verify_loiter_time();
|
|
break;
|
|
|
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
|
return verify_RTL();
|
|
break;
|
|
|
|
default:
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
|
|
return false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
static bool verify_condition_command() // Returns true if command complete
|
|
{
|
|
switch(non_nav_command_ID) {
|
|
case NO_COMMAND:
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_DELAY:
|
|
return verify_wait_delay();
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_DISTANCE:
|
|
return verify_within_distance();
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_CHANGE_ALT:
|
|
return verify_change_alt();
|
|
break;
|
|
|
|
default:
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
|
|
break;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Nav (Must) commands
|
|
/********************************************************************************/
|
|
|
|
static void do_RTL(void)
|
|
{
|
|
control_mode = RTL;
|
|
crash_timer = 0;
|
|
next_WP = home;
|
|
|
|
// Altitude to hold over home
|
|
// Set by configuration tool
|
|
// -------------------------
|
|
next_WP.alt = read_alt_to_hold();
|
|
|
|
if (g.log_bitmask & MASK_LOG_MODE)
|
|
Log_Write_Mode(control_mode);
|
|
}
|
|
|
|
static void do_takeoff()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
|
takeoff_pitch = (int)next_nav_command.p1 * 100;
|
|
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
|
|
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
|
|
takeoff_altitude = next_nav_command.alt;
|
|
//Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude);
|
|
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
|
|
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
|
|
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
|
|
}
|
|
|
|
static void do_nav_wp()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
}
|
|
|
|
static void do_land()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
}
|
|
|
|
static void do_loiter_unlimited()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
}
|
|
|
|
static void do_loiter_turns()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
loiter_total = next_nav_command.p1 * 360;
|
|
}
|
|
|
|
static void do_loiter_time()
|
|
{
|
|
set_next_WP(&next_nav_command);
|
|
loiter_time = millis();
|
|
loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Verify Nav (Must) commands
|
|
/********************************************************************************/
|
|
static bool verify_takeoff()
|
|
{
|
|
if (g_gps->ground_speed > 300){
|
|
if(hold_course == -1){
|
|
// save our current course to take off
|
|
if(g.compass_enabled) {
|
|
hold_course = dcm.yaw_sensor;
|
|
} else {
|
|
hold_course = g_gps->ground_course;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(hold_course > -1){
|
|
// recalc bearing error with hold_course;
|
|
nav_bearing = hold_course;
|
|
// recalc bearing error
|
|
calc_bearing_error();
|
|
}
|
|
|
|
if (current_loc.alt > takeoff_altitude) {
|
|
hold_course = -1;
|
|
takeoff_complete = true;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
static bool verify_land()
|
|
{
|
|
// we don't verify landing - we never go to a new Nav command after Land
|
|
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|
|
|| (current_loc.alt <= next_WP.alt + 300)){
|
|
|
|
land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude
|
|
|
|
if(hold_course == -1){
|
|
// save our current course to land
|
|
//hold_course = yaw_sensor;
|
|
// save the course line of the runway to land
|
|
hold_course = crosstrack_bearing;
|
|
}
|
|
}
|
|
|
|
if(hold_course > -1){
|
|
// recalc bearing error with hold_course;
|
|
nav_bearing = hold_course;
|
|
// recalc bearing error
|
|
calc_bearing_error();
|
|
}
|
|
|
|
update_crosstrack();
|
|
return false;
|
|
}
|
|
|
|
static bool verify_nav_wp()
|
|
{
|
|
hold_course = -1;
|
|
update_crosstrack();
|
|
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
|
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
|
|
return true;
|
|
}
|
|
// add in a more complex case
|
|
// Doug to do
|
|
if(loiter_sum > 300){
|
|
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool verify_loiter_unlim()
|
|
{
|
|
update_loiter();
|
|
calc_bearing_error();
|
|
return false;
|
|
}
|
|
|
|
static bool verify_loiter_time()
|
|
{
|
|
update_loiter();
|
|
calc_bearing_error();
|
|
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool verify_loiter_turns()
|
|
{
|
|
update_loiter();
|
|
calc_bearing_error();
|
|
if(loiter_sum > loiter_total) {
|
|
loiter_total = 0;
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
|
|
// clear the command queue;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool verify_RTL()
|
|
{
|
|
if (wp_distance <= g.waypoint_radius) {
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
|
|
return true;
|
|
}else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Condition (May) commands
|
|
/********************************************************************************/
|
|
|
|
static void do_wait_delay()
|
|
{
|
|
condition_start = millis();
|
|
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
|
|
}
|
|
|
|
static void do_change_alt()
|
|
{
|
|
condition_rate = next_nonnav_command.lat;
|
|
condition_value = next_nonnav_command.alt;
|
|
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
|
|
next_WP.alt = condition_value; // For future nav calculations
|
|
offset_altitude = 0; // For future nav calculations
|
|
}
|
|
|
|
static void do_within_distance()
|
|
{
|
|
condition_value = next_nonnav_command.lat;
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Verify Condition (May) commands
|
|
/********************************************************************************/
|
|
|
|
static bool verify_wait_delay()
|
|
{
|
|
if ((unsigned)(millis() - condition_start) > condition_value){
|
|
condition_value = 0;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool verify_change_alt()
|
|
{
|
|
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
|
|
condition_value = 0;
|
|
return true;
|
|
}
|
|
target_altitude += condition_rate / 10;
|
|
return false;
|
|
}
|
|
|
|
static bool verify_within_distance()
|
|
{
|
|
if (wp_distance < condition_value){
|
|
condition_value = 0;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/********************************************************************************/
|
|
// Do (Now) commands
|
|
/********************************************************************************/
|
|
|
|
static void do_loiter_at_location()
|
|
{
|
|
next_WP = current_loc;
|
|
}
|
|
|
|
static void do_jump()
|
|
{
|
|
struct Location temp;
|
|
if(next_nonnav_command.lat > 0) {
|
|
|
|
nav_command_ID = NO_COMMAND;
|
|
non_nav_command_ID = NO_COMMAND;
|
|
temp = get_cmd_with_index(g.command_index);
|
|
temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
|
|
|
|
set_cmd_with_index(temp, g.command_index);
|
|
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
|
|
} else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
|
|
nav_command_ID = NO_COMMAND;
|
|
non_nav_command_ID = NO_COMMAND;
|
|
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
|
|
}
|
|
}
|
|
|
|
static void do_change_speed()
|
|
{
|
|
// Note: we have no implementation for commanded ground speed, only air speed and throttle
|
|
if(next_nonnav_command.alt > 0)
|
|
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
|
|
|
|
if(next_nonnav_command.lat > 0)
|
|
g.throttle_cruise.set_and_save(next_nonnav_command.lat);
|
|
}
|
|
|
|
static void do_set_home()
|
|
{
|
|
if(next_nonnav_command.p1 == 1 && GPS_enabled) {
|
|
init_home();
|
|
} else {
|
|
home.id = MAV_CMD_NAV_WAYPOINT;
|
|
home.lng = next_nonnav_command.lng; // Lon * 10**7
|
|
home.lat = next_nonnav_command.lat; // Lat * 10**7
|
|
home.alt = max(next_nonnav_command.alt, 0);
|
|
home_is_set = true;
|
|
}
|
|
}
|
|
|
|
static void do_set_servo()
|
|
{
|
|
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
|
|
}
|
|
|
|
static void do_set_relay()
|
|
{
|
|
if (next_nonnav_command.p1 == 1) {
|
|
relay.on();
|
|
} else if (next_nonnav_command.p1 == 0) {
|
|
relay.off();
|
|
}else{
|
|
relay.toggle();
|
|
}
|
|
}
|
|
|
|
static void do_repeat_servo()
|
|
{
|
|
event_id = next_nonnav_command.p1 - 1;
|
|
|
|
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
|
|
|
|
event_timer = 0;
|
|
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
|
event_repeat = next_nonnav_command.lat * 2;
|
|
event_value = next_nonnav_command.alt;
|
|
|
|
switch(next_nonnav_command.p1) {
|
|
case CH_5:
|
|
event_undo_value = g.rc_5.radio_trim;
|
|
break;
|
|
case CH_6:
|
|
event_undo_value = g.rc_6.radio_trim;
|
|
break;
|
|
case CH_7:
|
|
event_undo_value = g.rc_7.radio_trim;
|
|
break;
|
|
case CH_8:
|
|
event_undo_value = g.rc_8.radio_trim;
|
|
break;
|
|
}
|
|
update_events();
|
|
}
|
|
}
|
|
|
|
static void do_repeat_relay()
|
|
{
|
|
event_id = RELAY_TOGGLE;
|
|
event_timer = 0;
|
|
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
|
event_repeat = next_nonnav_command.alt * 2;
|
|
update_events();
|
|
}
|