From e2a7ba5b0cb3db7f1f8deff224bd83ad6843cb45 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 24 Aug 2016 23:08:30 -0400 Subject: [PATCH] Sub: replace land with surface mode --- ArduSub/ArduSub.cpp | 2 +- ArduSub/GCS_Mavlink.cpp | 57 ++++++++++--------- ArduSub/Sub.h | 12 +--- ArduSub/commands_logic.cpp | 19 +------ ArduSub/control_surface.cpp | 64 +++++++++++++++++++++ ArduSub/defines.h | 5 +- ArduSub/ekf_check.cpp | 72 +++++++++++------------ ArduSub/events.cpp | 110 ++++++++++++++++++------------------ ArduSub/fence.cpp | 32 +++++------ ArduSub/flight_mode.cpp | 14 ++--- ArduSub/landing_gear.cpp | 55 +++++++++--------- ArduSub/switches.cpp | 18 +++--- 12 files changed, 256 insertions(+), 204 deletions(-) create mode 100644 ArduSub/control_surface.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 670eae024f..b11ee21c2b 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -396,7 +396,7 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { DataFlash.Log_Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { + if (should_log(MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) { Log_Write_Nav_Tuning(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0c148ef713..2400be58c6 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -170,7 +170,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) case LOITER: case RTL: case CIRCLE: - case LAND: + case SURFACE: case OF_LOITER: case POSHOLD: case BRAKE: @@ -1278,11 +1278,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; - case MAV_CMD_NAV_LAND: - if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { - result = MAV_RESULT_ACCEPTED; - } - break; +// Not supported in sub +// case MAV_CMD_NAV_LAND: +// if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { +// result = MAV_RESULT_ACCEPTED; +// } +// break; case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] @@ -1652,28 +1653,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - /* Solo user holds down Fly button for a couple of seconds */ - case MAV_CMD_SOLO_BTN_FLY_HOLD: { - result = MAV_RESULT_ACCEPTED; +// Not supported in Sub - if (sub.failsafe.radio) { - break; - } - - if (!sub.motors.armed()) { - // if disarmed, arm motors - sub.init_arm_motors(true); - } else if (sub.ap.land_complete) { - // if armed and landed, takeoff - if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { - sub.do_user_takeoff(packet.param1*100, true); - } - } else { - // if flying, land - sub.set_mode(LAND, MODE_REASON_GCS_COMMAND); - } - break; - } +// /* Solo user holds down Fly button for a couple of seconds */ +// case MAV_CMD_SOLO_BTN_FLY_HOLD: { +// result = MAV_RESULT_ACCEPTED; +// +// if (sub.failsafe.radio) { +// break; +// } +// +// if (!sub.motors.armed()) { +// // if disarmed, arm motors +// sub.init_arm_motors(true); +// } else if (sub.ap.land_complete) { +// // if armed and landed, takeoff +// if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { +// sub.do_user_takeoff(packet.param1*100, true); +// } +// } else { +// // if flying, land +// sub.set_mode(LAND, MODE_REASON_GCS_COMMAND); +// } +// break; +// } /* Solo user presses pause button */ case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 9b107fc93c..2f77f66a02 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -791,16 +791,7 @@ 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 land_init(bool ignore_checks); - void land_run(); - void land_gps_run(); - void land_nogps_run(); - void land_run_vertical_control(bool pause_descent = false); - void land_run_horizontal_control(); float get_land_descent_speed(); - void land_do_not_use_GPS(); - void set_mode_land_with_pause(mode_reason_t reason); - bool landing_with_GPS(); bool loiter_init(bool ignore_checks); void loiter_run(); bool poshold_init(bool ignore_checks); @@ -1054,6 +1045,9 @@ private: void set_leak_status(bool status); + bool surface_init(bool ignore_flags); + void surface_run(); + public: void mavlink_delay_cb(); void failsafe_check(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 571b1e5aed..4bdaadb776 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -246,23 +246,8 @@ void Sub::exit_mission() { // play a tone AP_Notify::events.mission_complete = 1; - // if we are not on the ground switch to loiter or land - if(!ap.land_complete) { - // try to enter loiter but if that fails land - if(!auto_loiter_start()) { - set_mode(LAND, MODE_REASON_MISSION_END); - } - }else{ -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.throttle_zero || failsafe.radio) { - init_disarm_motors(); - } -#else - // if we've landed it's safe to disarm - init_disarm_motors(); -#endif - } + + init_disarm_motors(); } /********************************************************************************/ diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp new file mode 100644 index 0000000000..98a6a3d446 --- /dev/null +++ b/ArduSub/control_surface.cpp @@ -0,0 +1,64 @@ +#include "Sub.h" + + +bool Sub::surface_init(bool ignore_checks) +{ + + if(!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately. + gcs_send_text(MAV_SEVERITY_WARNING, "Surface mode requires external pressure sensor."); + return false; + } + + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); + pos_control.set_accel_z(wp_nav.get_accel_z()); + + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + + return true; + +} + +void Sub::surface_run() +{ + float target_roll, target_pitch; + float target_yaw_rate; + + // if not armed set throttle to zero and exit immediately + if (!motors.armed() || !motors.get_interlock()) { + motors.output_min(); + motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); + return; + } + + // Already at surface, hold depth at surface + if(ap.at_surface) + set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE); + + // convert pilot input to lean angles + // To-Do: convert get_pilot_desired_lean_angles to return angles as floats + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + + // set target climb rate + float cmb_rate = constrain_float(abs(g.land_speed), 1, pos_control.get_speed_up()); + + // record desired climb rate for logging + desired_climb_rate = cmb_rate; + + // update altitude target and call position controller + pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); + pos_control.update_z_controller(); + + // pilot has control for repositioning + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} diff --git a/ArduSub/defines.h b/ArduSub/defines.h index eb3e830893..7819d2ca45 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -105,7 +105,7 @@ enum control_mode_t { LOITER = 5, // automatic horizontal acceleration with automatic throttle RTL = 6, // automatic return to launching point CIRCLE = 7, // automatic circular flight with automatic throttle - LAND = 9, // automatic landing with horizontal position control + SURFACE = 9, // automatic landing with horizontal position control OF_LOITER = 10, // deprecated DRIFT = 11, // semi-automous position, yaw and throttle control SPORT = 13, // manual earth-frame angular rate control with manual throttle @@ -131,7 +131,8 @@ enum mode_reason_t { MODE_REASON_FENCE_BREACH, MODE_REASON_TERRAIN_FAILSAFE, MODE_REASON_BRAKE_TIMEOUT, - MODE_REASON_FLIP_COMPLETE + MODE_REASON_FLIP_COMPLETE, + MODE_REASON_SURFACE_COMPLETE }; // Tuning enumeration diff --git a/ArduSub/ekf_check.cpp b/ArduSub/ekf_check.cpp index 6f0469ca19..446d040c0b 100644 --- a/ArduSub/ekf_check.cpp +++ b/ArduSub/ekf_check.cpp @@ -118,42 +118,42 @@ bool Sub::ekf_over_threshold() // failsafe_ekf_event - perform ekf failsafe void Sub::failsafe_ekf_event() { - // return immediately if ekf failsafe already triggered - if (failsafe.ekf) { - return; - } - - // do nothing if motors disarmed - if (!motors.armed()) { - return; - } - - // do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize - if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { - return; - } - - // EKF failsafe event has occurred - failsafe.ekf = true; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED); - - // take action based on fs_ekf_action parameter - switch (g.fs_ekf_action) { - case FS_EKF_ACTION_ALTHOLD: - // AltHold - if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { - set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); - } - break; - default: - set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); - break; - } - - // if flight mode is already LAND ensure it's not the GPS controlled LAND - if (control_mode == LAND) { - land_do_not_use_GPS(); - } +// // return immediately if ekf failsafe already triggered +// if (failsafe.ekf) { +// return; +// } +// +// // do nothing if motors disarmed +// if (!motors.armed()) { +// return; +// } +// +// // do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize +// if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { +// return; +// } +// +// // EKF failsafe event has occurred +// failsafe.ekf = true; +// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED); +// +// // take action based on fs_ekf_action parameter +// switch (g.fs_ekf_action) { +// case FS_EKF_ACTION_ALTHOLD: +// // AltHold +// if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { +// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); +// } +// break; +// default: +// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); +// break; +// } +// +// // if flight mode is already LAND ensure it's not the GPS controlled LAND +// if (control_mode == LAND) { +// land_do_not_use_GPS(); +// } } // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index 719c4da463..f97d8e28c6 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -8,29 +8,29 @@ */ void Sub::failsafe_radio_on_event() { - // if motors are not armed there is nothing to do - if( !motors.armed() ) { - return; - } - - if (should_disarm_on_failsafe()) { - init_disarm_motors(); - } else { - if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { - // continue mission - } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { - // continue landing - } else { - if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { - set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); - } else { - set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); - } - } - } - - // log the error to the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED); +// // if motors are not armed there is nothing to do +// if( !motors.armed() ) { +// return; +// } +// +// if (should_disarm_on_failsafe()) { +// init_disarm_motors(); +// } else { +// if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { +// // continue mission +// } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { +// // continue landing +// } else { +// if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { +// set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); +// } else { +// set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); +// } +// } +// } +// +// // log the error to the dataflash +// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED); } @@ -46,30 +46,30 @@ void Sub::failsafe_radio_off_event() void Sub::failsafe_battery_event(void) { - // return immediately if low battery event has already been triggered - if (failsafe.battery) { - return; - } - - // failsafe check - if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { - if (should_disarm_on_failsafe()) { - init_disarm_motors(); - } else { - if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { - set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - } else { - set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - } - } - } - - // set the low battery flag - set_failsafe_battery(true); - - // warn the ground station and log to dataflash - gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); +// // return immediately if low battery event has already been triggered +// if (failsafe.battery) { +// return; +// } +// +// // failsafe check +// if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { +// if (should_disarm_on_failsafe()) { +// init_disarm_motors(); +// } else { +// if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { +// set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); +// } else { +// set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); +// } +// } +// } +// +// // set the low battery flag +// set_failsafe_battery(true); +// +// // warn the ground station and log to dataflash +// gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); +// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); } @@ -195,14 +195,14 @@ void Sub::failsafe_terrain_on_event() // this is always called from a failsafe so we trigger notification to pilot void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason) { - // attempt to switch to RTL, if this fails then switch to Land - if (!set_mode(RTL, reason)) { - // set mode to land will trigger mode change notification to pilot - set_mode_land_with_pause(reason); - } else { - // alert pilot to mode change - AP_Notify::events.failsafe_mode_change = 1; - } +// // attempt to switch to RTL, if this fails then switch to Land +// if (!set_mode(RTL, reason)) { +// // set mode to land will trigger mode change notification to pilot +// set_mode_land_with_pause(reason); +// } else { +// // alert pilot to mode change +// AP_Notify::events.failsafe_mode_change = 1; +// } } bool Sub::should_disarm_on_failsafe() { diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index a076f61eb1..3370fdf8f8 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -29,22 +29,22 @@ void Sub::fence_check() // if the user wants some kind of response and motors are armed if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) { - - // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle - // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down - if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ - init_disarm_motors(); - }else{ - // if we are within 100m of the fence, RTL - if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) { - set_mode(LAND, MODE_REASON_FENCE_BREACH); - } - }else{ - // if more than 100m outside the fence just force a land - set_mode(LAND, MODE_REASON_FENCE_BREACH); - } - } +// +// // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle +// // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down +// if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ +// init_disarm_motors(); +// }else{ +// // if we are within 100m of the fence, RTL +// if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { +// if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) { +// set_mode(LAND, MODE_REASON_FENCE_BREACH); +// } +// }else{ +// // if more than 100m outside the fence just force a land +// set_mode(LAND, MODE_REASON_FENCE_BREACH); +// } +// } } // log an error in the dataflash diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 1e120ba28b..b942eb6f8c 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -55,8 +55,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) success = guided_init(ignore_checks); break; - case LAND: - success = land_init(ignore_checks); + case SURFACE: + success = surface_init(ignore_checks); break; case RTL: @@ -172,8 +172,8 @@ void Sub::update_flight_mode() guided_run(); break; - case LAND: - land_run(); + case SURFACE: + surface_run(); break; case RTL: @@ -304,7 +304,7 @@ void Sub::notify_flight_mode(control_mode_t mode) { case GUIDED: case RTL: case CIRCLE: - case LAND: + case SURFACE: // autopilot modes AP_Notify::flags.autopilot_mode = true; break; @@ -345,8 +345,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case CIRCLE: port->print("CIRCLE"); break; - case LAND: - port->print("LAND"); + case SURFACE: + port->print("SURFACE"); break; case OF_LOITER: port->print("OF_LOITER"); diff --git a/ArduSub/landing_gear.cpp b/ArduSub/landing_gear.cpp index f52e3881ba..aa9b19a4db 100644 --- a/ArduSub/landing_gear.cpp +++ b/ArduSub/landing_gear.cpp @@ -6,32 +6,35 @@ // Run landing gear controller at 10Hz void Sub::landinggear_update(){ - // If landing gear control is active, run update function. - if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ + // Do nothing for Sub - // last status (deployed or retracted) used to check for changes - static bool last_deploy_status; - // if we are doing an automatic landing procedure, force the landing gear to deploy. - // To-Do: should we pause the auto-land procedure to give time for gear to come down? - if (control_mode == LAND || - (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || - (control_mode == AUTO && auto_mode == Auto_Land) || - (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { - landinggear.force_deploy(true); - } - - landinggear.update(); - - // send event message to datalog if status has changed - if (landinggear.deployed() != last_deploy_status){ - if (landinggear.deployed()) { - Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); - } else { - Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); - } - } - - last_deploy_status = landinggear.deployed(); - } +// // If landing gear control is active, run update function. +// if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ +// +// // last status (deployed or retracted) used to check for changes +// static bool last_deploy_status; +// +// // if we are doing an automatic landing procedure, force the landing gear to deploy. +// // To-Do: should we pause the auto-land procedure to give time for gear to come down? +// if (control_mode == LAND || +// (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || +// (control_mode == AUTO && auto_mode == Auto_Land) || +// (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { +// landinggear.force_deploy(true); +// } +// +// landinggear.update(); +// +// // send event message to datalog if status has changed +// if (landinggear.deployed() != last_deploy_status){ +// if (landinggear.deployed()) { +// Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); +// } else { +// Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); +// } +// } +// +// last_deploy_status = landinggear.deployed(); +// } } diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index abfc3d61ac..21dfc135d2 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -453,14 +453,16 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) #endif case AUXSW_LAND: - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(LAND, MODE_REASON_TX_COMMAND); - }else{ - // return to flight mode switch's flight mode if we are currently in LAND - if (control_mode == LAND) { - reset_control_switch(); - } - } + // Do nothing for Sub + +// if (ch_flag == AUX_SWITCH_HIGH) { +// set_mode(LAND, MODE_REASON_TX_COMMAND); +// }else{ +// // return to flight mode switch's flight mode if we are currently in LAND +// if (control_mode == LAND) { +// reset_control_switch(); +// } +// } break; #if PARACHUTE == ENABLED