diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 5632bd6cdb..b7f78ec98b 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -49,11 +49,12 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) switch (control_mode) { case AUTO: case RTL: - case LOITER: + case VELHOLD: case GUIDED: case CIRCLE: case POSHOLD: case BRAKE: + case TRANSECT: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what // APM does in any mode, as that is defined as "system finds its own goal @@ -167,19 +168,17 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) case ALT_HOLD: case AUTO: case GUIDED: - case LOITER: + case VELHOLD: case RTL: case CIRCLE: case SURFACE: case OF_LOITER: case POSHOLD: case BRAKE: + case TRANSECT: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; break; - case SPORT: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; - break; default: break; } @@ -1267,7 +1266,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_UNLIM: - if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { + if (sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) { result = MAV_RESULT_ACCEPTED; } break; @@ -1638,78 +1637,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - /* Solo user presses Fly button */ - case MAV_CMD_SOLO_BTN_FLY_CLICK: { - result = MAV_RESULT_ACCEPTED; - - if (sub.failsafe.radio) { - break; - } - - // set mode to Loiter or fall back to AltHold - if (!sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { - sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND); - } - break; - } - -// Not supported in Sub - -// /* 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: { - result = MAV_RESULT_ACCEPTED; - - if (sub.failsafe.radio) { - break; - } - - if (sub.motors.armed()) { - if (sub.ap.land_complete) { - // if landed, disarm motors - sub.init_disarm_motors(); - } 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) && sub.control_mode == GUIDED); - - if (!shot_mode) { - if (sub.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) { - sub.brake_timeout_to_loiter_ms(2500); - } else { - sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND); - } - } else { - // SoloLink is expected to handle pause in shots - } - } - } - break; - } - - default: result = MAV_RESULT_UNSUPPORTED; break; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 368c8c9446..9d7e34d1d3 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -792,8 +792,8 @@ private: void guided_limit_init_time_and_pos(); bool guided_limit_check(); float get_land_descent_speed(); - bool loiter_init(bool ignore_checks); - void loiter_run(); + bool velhold_init(bool ignore_checks); + void velhold_run(); bool poshold_init(bool ignore_checks); void poshold_run(); void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); @@ -826,8 +826,8 @@ private: void rtl_land_run(); void rtl_build_path(bool terrain_following_allowed); void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed); - bool sport_init(bool ignore_checks); - void sport_run(); + bool transect_init(bool ignore_checks); + void transect_run(); bool stabilize_init(bool ignore_checks); void stabilize_run(); bool manual_init(bool ignore_checks); diff --git a/ArduSub/control_brake.cpp b/ArduSub/control_brake.cpp index 27c7adac5b..3deebdcfd9 100644 --- a/ArduSub/control_brake.cpp +++ b/ArduSub/control_brake.cpp @@ -74,7 +74,7 @@ void Sub::brake_run() pos_control.update_z_controller(); if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) { - if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) { + if (!set_mode(POSHOLD, MODE_REASON_BRAKE_TIMEOUT)) { set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT); } } diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index dd85982e4f..758bd26d79 100644 --- a/ArduSub/control_rtl.cpp +++ b/ArduSub/control_rtl.cpp @@ -287,7 +287,7 @@ void Sub::rtl_descent_run() if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + if (!set_mode(POSHOLD, MODE_REASON_THROTTLE_LAND_ESCAPE)) { set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } diff --git a/ArduSub/control_sport.cpp b/ArduSub/control_transect.cpp similarity index 100% rename from ArduSub/control_sport.cpp rename to ArduSub/control_transect.cpp diff --git a/ArduSub/control_loiter.cpp b/ArduSub/control_velhold.cpp similarity index 100% rename from ArduSub/control_loiter.cpp rename to ArduSub/control_velhold.cpp diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 57bca7c5d7..c138614b12 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -102,13 +102,13 @@ enum control_mode_t { ALT_HOLD = 2, // manual airframe angle with automatic throttle AUTO = 3, // fully automatic waypoint control using mission commands GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands - LOITER = 5, // automatic horizontal acceleration with automatic throttle + VELHOLD = 5, // automatic horizontal acceleration with automatic throttle RTL = 6, // automatic return to launching point CIRCLE = 7, // automatic circular flight with automatic throttle 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 + TRANSECT = 13, // manual earth-frame angular rate control with manual throttle FLIP = 14, // automatically flip the vehicle on the roll axis AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains POSHOLD = 16, // automatic position hold with manual override, with automatic throttle diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index b942eb6f8c..4a8cf2ccc0 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -47,8 +47,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) success = circle_init(ignore_checks); break; - case LOITER: - success = loiter_init(ignore_checks); + case VELHOLD: + success = velhold_init(ignore_checks); break; case GUIDED: @@ -67,8 +67,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) success = drift_init(ignore_checks); break; - case SPORT: - success = sport_init(ignore_checks); + case TRANSECT: + success = transect_init(ignore_checks); break; case FLIP: @@ -164,8 +164,8 @@ void Sub::update_flight_mode() circle_run(); break; - case LOITER: - loiter_run(); + case VELHOLD: + velhold_run(); break; case GUIDED: @@ -184,8 +184,8 @@ void Sub::update_flight_mode() drift_run(); break; - case SPORT: - sport_run(); + case TRANSECT: + transect_run(); break; case FLIP: @@ -259,13 +259,14 @@ bool Sub::mode_requires_GPS(control_mode_t mode) { switch(mode) { case AUTO: case GUIDED: - case LOITER: + case VELHOLD: case RTL: case CIRCLE: case DRIFT: case POSHOLD: case BRAKE: case THROW: + case TRANSECT: return true; default: return false; @@ -291,7 +292,7 @@ bool Sub::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 Sub::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 == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == TRANSECT || mode == THROW || (arming_from_gcs && mode == GUIDED)) { return true; } return false; @@ -336,8 +337,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case GUIDED: port->print("GUIDED"); break; - case LOITER: - port->print("LOITER"); + case VELHOLD: + port->print("VELHOLD"); break; case RTL: port->print("RTL"); @@ -354,8 +355,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case DRIFT: port->print("DRIFT"); break; - case SPORT: - port->print("SPORT"); + case TRANSECT: + port->print("TRANSECT"); break; case FLIP: port->print("FLIP"); diff --git a/ArduSub/navigation.cpp b/ArduSub/navigation.cpp index 47668d3bf9..402e60b4f3 100644 --- a/ArduSub/navigation.cpp +++ b/ArduSub/navigation.cpp @@ -26,7 +26,7 @@ void Sub::calc_distance_and_bearing() void Sub::calc_wp_distance() { // get target from loiter or wpinav controller - if (control_mode == LOITER || control_mode == CIRCLE) { + if (control_mode == CIRCLE) { wp_distance = wp_nav.get_loiter_distance_to_target(); }else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) { wp_distance = wp_nav.get_wp_distance_to_destination(); @@ -39,7 +39,7 @@ void Sub::calc_wp_distance() void Sub::calc_wp_bearing() { // get target from loiter or wpinav controller - if (control_mode == LOITER || control_mode == CIRCLE) { + if (control_mode == CIRCLE) { wp_bearing = wp_nav.get_loiter_bearing_to_target(); } else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) { wp_bearing = wp_nav.get_wp_bearing_to_destination(); diff --git a/ArduSub/takeoff.cpp b/ArduSub/takeoff.cpp index ba5b8dcbe1..cc3da0f9a1 100644 --- a/ArduSub/takeoff.cpp +++ b/ArduSub/takeoff.cpp @@ -11,43 +11,13 @@ // must_nagivate is true if mode must also control horizontal position bool Sub::current_mode_has_user_takeoff(bool must_navigate) { - switch (control_mode) { - case GUIDED: - case LOITER: - case POSHOLD: - return true; - case ALT_HOLD: - case SPORT: - return !must_navigate; - default: - return false; - } + return false; // not supported in Sub } // initiate user takeoff - called when MAVLink TAKEOFF command is received bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) { - if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) { - - switch(control_mode) { - case GUIDED: - if (guided_takeoff_start(takeoff_alt_cm)) { - set_auto_armed(true); - return true; - } - return false; - case LOITER: - case POSHOLD: - case ALT_HOLD: - case SPORT: - set_auto_armed(true); - takeoff_timer_start(takeoff_alt_cm); - return true; - default: - return false; - } - } - return false; + return false; // not supported in Sub } // start takeoff to specified altitude above home in centimeters