From caf5e5b7c54a92616fe1b46af3083f1af13c89ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Mar 2013 08:53:20 +1100 Subject: [PATCH] Rover: added HOLD mode used when RTL completes --- APMrover2/APMrover2.pde | 9 ++++++++- APMrover2/GCS_Mavlink.pde | 5 +++++ APMrover2/Parameters.pde | 12 ++++++------ APMrover2/commands_logic.pde | 4 ++-- APMrover2/defines.h | 1 + APMrover2/events.pde | 1 + APMrover2/setup.pde | 1 + APMrover2/system.pde | 4 ++++ 8 files changed, 28 insertions(+), 9 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index d76d591116..9f487aaeb8 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -863,6 +863,12 @@ static void update_current_mode(void) g.channel_steer.servo_out = g.channel_steer.pwm_to_angle(); break; + case HOLD: + // hold position - stop motors and center steering + g.channel_throttle.servo_out = 0; + g.channel_steer.servo_out = 0; + break; + case INITIALISING: break; } @@ -872,6 +878,7 @@ static void update_navigation() { switch (control_mode) { case MANUAL: + case HOLD: case LEARNING: case STEERING: case INITIALISING: @@ -888,7 +895,7 @@ static void update_navigation() calc_bearing_error(); if (verify_RTL()) { g.channel_throttle.servo_out = g.throttle_min.get(); - set_mode(MANUAL); + set_mode(HOLD); } break; } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 12a91cd971..30aa3d9285 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -54,6 +54,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) case INITIALISING: system_status = MAV_STATE_CALIBRATING; break; + case HOLD: + system_status = 0; + break; } #if ENABLE_STICK_MIXING==ENABLED @@ -132,6 +135,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack switch (control_mode) { case MANUAL: + case HOLD: break; case LEARNING: @@ -1074,6 +1078,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } switch (packet.custom_mode) { case MANUAL: + case HOLD: case LEARNING: case STEERING: case AUTO: diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 39da28f8c4..8e2b7a7a80 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -307,7 +307,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) GSCALAR(mode1, "MODE1", MODE_1), @@ -315,35 +315,35 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: MODE2 // @DisplayName: Mode2 // @Description: Driving mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", MODE_2), // @Param: MODE3 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", MODE_3), // @Param: MODE4 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", MODE_4), // @Param: MODE5 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", MODE_5), // @Param: MODE6 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", MODE_6), diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index a7f1ff9d3c..8a1fc53432 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -109,8 +109,8 @@ static void handle_process_do_command() static void handle_no_commands() { - gcs_send_text_fmt(PSTR("No commands - setting MANUAL")); - set_mode(MANUAL); + gcs_send_text_fmt(PSTR("No commands - setting HOLD")); + set_mode(HOLD); } /********************************************************************************/ diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 6836a2225f..05e97f762f 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -66,6 +66,7 @@ enum mode { MANUAL=0, LEARNING=2, STEERING=3, + HOLD=4, AUTO=10, RTL=11, GUIDED=15, diff --git a/APMrover2/events.pde b/APMrover2/events.pde index da8416959f..15d7974fb3 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -21,6 +21,7 @@ static void failsafe_long_on_event(int fstype) break; case RTL: + case HOLD: default: break; } diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 1b87eee881..84eb7d810a 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -338,6 +338,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) while ( mode != MANUAL && + mode != HOLD && mode != LEARNING && mode != STEERING && mode != AUTO && diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 8b27834439..22846abf0b 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -314,6 +314,7 @@ static void set_mode(enum mode mode) switch(control_mode) { case MANUAL: + case HOLD: case LEARNING: case STEERING: break; @@ -495,6 +496,9 @@ print_mode(uint8_t mode) case MANUAL: cliSerial->println_P(PSTR("Manual")); break; + case HOLD: + cliSerial->println_P(PSTR("HOLD")); + break; case LEARNING: cliSerial->println_P(PSTR("Learning")); break;