From 1161417d7f8f759d97f6e2f3a625ecd608dae929 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Aug 2016 17:44:12 +0900 Subject: [PATCH] Copter: add GUIDED_NOGPS flight mode This mode is a cut down version of Guided mode that only accepts attitude commands. This mode does not require a GPS lock --- ArduCopter/Copter.h | 2 ++ ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/Parameters.cpp | 12 ++++++------ ArduCopter/arming_checks.cpp | 2 +- ArduCopter/control_guided_nogps.cpp | 24 ++++++++++++++++++++++++ ArduCopter/defines.h | 1 + ArduCopter/events.cpp | 2 +- ArduCopter/flight_mode.cpp | 14 +++++++++++++- 8 files changed, 49 insertions(+), 10 deletions(-) create mode 100644 ArduCopter/control_guided_nogps.cpp diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c3292f4a70..8b62917404 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -817,6 +817,8 @@ private: void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); void guided_limit_init_time_and_pos(); bool guided_limit_check(); + bool guided_nogps_init(bool ignore_checks); + void guided_nogps_run(); bool land_init(bool ignore_checks); void land_run(); void land_gps_run(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 651e4c6f5a..ed81ead313 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1624,7 +1624,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } else { // assume that shots modes are all done in guided. // NOTE: this may need to change if we add a non-guided shot mode - bool shot_mode = (!is_zero(packet.param1) && copter.control_mode == GUIDED); + bool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == GUIDED || copter.control_mode == GUIDED_NOGPS)); if (!shot_mode) { if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 577bc9a02e..a5c137ba9a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -306,42 +306,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: Flight Mode 2 // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: Flight Mode 3 // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: Flight Mode 4 // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: Flight Mode 5 // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: Flight Mode 6 // @Description: Flight mode when Channel 5 pwm is >=1750 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 7e47fee542..a415b57f81 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -714,7 +714,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } // check throttle is not too high - skips checks if arming from GCS in Guided - if (!(arming_from_gcs && control_mode == GUIDED)) { + if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) { // above top of deadband is too always high if (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) { if (display_failure) { diff --git a/ArduCopter/control_guided_nogps.cpp b/ArduCopter/control_guided_nogps.cpp new file mode 100644 index 0000000000..5f6a70b1fe --- /dev/null +++ b/ArduCopter/control_guided_nogps.cpp @@ -0,0 +1,24 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + +/* + * Init and run calls for guided_nogps flight mode + */ + +// initialise guided_nogps controller +bool Copter::guided_nogps_init(bool ignore_checks) +{ + // start in angle control mode + guided_angle_control_start(); + return true; +} + +// guided_run - runs the guided controller +// should be called at 100hz or more +void Copter::guided_nogps_run() +{ + // run angle controller + guided_angle_control_run(); +} + diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7e38111768..9d60b730e1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -106,6 +106,7 @@ enum control_mode_t { BRAKE = 17, // full-brake using inertial/GPS system, no pilot input THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft + GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude }; enum mode_reason_t { diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 55c0ed6188..0d80e59b9d 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -135,7 +135,7 @@ void Copter::failsafe_gcs_off_event(void) void Copter::failsafe_terrain_check() { // trigger with 5 seconds of failures while in AUTO mode - bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL); + bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == GUIDED_NOGPS || control_mode == RTL); bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; bool trigger_event = valid_mode && timeout; diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 9556fa4cdf..1811e0c006 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -107,6 +107,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) success = avoid_adsb_init(ignore_checks); break; + case GUIDED_NOGPS: + success = guided_nogps_init(ignore_checks); + break; + default: success = false; break; @@ -234,6 +238,10 @@ void Copter::update_flight_mode() avoid_adsb_run(); break; + case GUIDED_NOGPS: + guided_nogps_run(); + break; + default: break; } @@ -328,7 +336,7 @@ bool Copter::mode_has_manual_throttle(control_mode_t mode) { // mode_allows_arming - returns true if vehicle can be armed in the specified mode // arming_from_gcs should be set to true if the arming request comes from the ground station bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) { - if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) { + if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && (mode == GUIDED || mode == GUIDED_NOGPS))) { return true; } return false; @@ -342,6 +350,7 @@ void Copter::notify_flight_mode(control_mode_t mode) { case RTL: case CIRCLE: case AVOID_ADSB: + case GUIDED_NOGPS: case LAND: // autopilot modes AP_Notify::flags.autopilot_mode = true; @@ -410,6 +419,9 @@ void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case AVOID_ADSB: port->print("AVOID_ADSB"); break; + case GUIDED_NOGPS: + port->print("GUIDED_NOGPS"); + break; default: port->printf("Mode(%u)", (unsigned)mode); break;