diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0b670f5fb5..52662edabb 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -581,6 +581,7 @@ void Plane::update_flight_mode(void) handle_auto_mode(); break; + case AVOID_ADSB: case GUIDED: if (auto_state.vtol_loiter && quadplane.available()) { quadplane.guided_update(); @@ -806,6 +807,7 @@ void Plane::update_navigation() // fall through to LOITER case LOITER: + case AVOID_ADSB: case GUIDED: update_loiter(radius); break; diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 10cbd1fe7c..b3e6f027be 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -432,7 +432,7 @@ void Plane::calc_throttle() int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand(); // Received an external msg that guides throttle in the last 3 seconds? - if (control_mode == GUIDED && + if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && plane.guided_state.last_forced_throttle_ms > 0 && millis() - plane.guided_state.last_forced_throttle_ms < 3000) { commanded_throttle = plane.guided_state.forced_throttle; @@ -458,7 +458,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) int16_t commanded_rudder; // Received an external msg that guides yaw in the last 3 seconds? - if (control_mode == GUIDED && + if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && plane.guided_state.last_forced_rpy_ms.z > 0 && millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) { commanded_rudder = plane.guided_state.forced_rpy_cd.z; @@ -542,7 +542,7 @@ void Plane::calc_nav_pitch() int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand(); // Received an external msg that guides roll in the last 3 seconds? - if (control_mode == GUIDED && + if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && plane.guided_state.last_forced_rpy_ms.y > 0 && millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { commanded_pitch = plane.guided_state.forced_rpy_cd.y; @@ -560,7 +560,7 @@ void Plane::calc_nav_roll() int32_t commanded_roll = nav_controller->nav_roll_cd(); // Received an external msg that guides roll in the last 3 seconds? - if (control_mode == GUIDED && + if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && plane.guided_state.last_forced_rpy_ms.x > 0 && millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { commanded_roll = plane.guided_state.forced_rpy_cd.x; @@ -1064,7 +1064,7 @@ void Plane::set_servos(void) // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set channel_throttle->set_radio_out(channel_throttle->get_radio_in()); - } else if (control_mode == GUIDED && + } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->set_radio_out(channel_throttle->get_radio_in()); @@ -1342,6 +1342,7 @@ bool Plane::allow_reverse_thrust(void) case FLY_BY_WIRE_B: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB); break; + case AVOID_ADSB: case GUIDED: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED); break; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 68fdaed0d9..301892ffde 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -52,6 +52,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case AUTO: case RTL: case LOITER: + case AVOID_ADSB: case GUIDED: case CIRCLE: case QRTL: @@ -209,6 +210,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case AUTO: case RTL: case LOITER: + case AVOID_ADSB: case GUIDED: case CIRCLE: case QRTL: @@ -1242,7 +1244,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // this command should be ignored since it comes in from GCS // or a companion computer: result = MAV_RESULT_FAILED; - if (plane.control_mode != GUIDED && plane.control_mode != AUTO) { + if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) { // failed break; } @@ -2122,7 +2124,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // in e.g., RTL, CICLE. Specifying a single mode for companion // computer control is more safe (even more so when using // FENCE_ACTION = 4 for geofence failures). - if (plane.control_mode != GUIDED) { // don't screw up failsafes + if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { // don't screw up failsafes break; } @@ -2220,7 +2222,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode // for companion computer control is more safe (provided // one uses the FENCE_ACTION = 4 (RTL) for geofence failures). - if (plane.control_mode != GUIDED) { + if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { //don't screw up failsafes break; } diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 3548a53dc7..53c9d4555a 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -75,6 +75,7 @@ void Plane::setup_glide_slope(void) */ switch (control_mode) { case RTL: + case AVOID_ADSB: case GUIDED: /* glide down slowly if above target altitude, but ascend more rapidly if below it. See diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 723b03b25b..aa1eb48379 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -58,6 +58,7 @@ enum FlightMode { AUTO = 10, RTL = 11, LOITER = 12, + AVOID_ADSB = 14, GUIDED = 15, INITIALISING = 16, QSTABILIZE = 17, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index b12ced87fe..10a99f47f1 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -36,6 +36,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) break; case AUTO: + case AVOID_ADSB: case GUIDED: case LOITER: if(g.short_fs_action != 0) { @@ -95,6 +96,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) break; case AUTO: + case AVOID_ADSB: case GUIDED: case LOITER: if(g.long_fs_action == 3) { diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 6fca7d22f0..b9d9f9c5ed 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -285,7 +285,7 @@ void Plane::geofence_check(bool altitude_check_only) // GUIDED to the return point if (geofence_state != NULL && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) && - control_mode == GUIDED && + (control_mode == GUIDED || control_mode == AVOID_ADSB) && geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && @@ -348,7 +348,7 @@ void Plane::geofence_check(bool altitude_check_only) // we are outside the fence if (geofence_state->fence_triggered && - (control_mode == GUIDED || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) { + (control_mode == GUIDED || control_mode == AVOID_ADSB || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; @@ -432,7 +432,7 @@ bool Plane::geofence_stickmixing(void) { if (geofence_enabled() && geofence_state != NULL && geofence_state->fence_triggered && - control_mode == GUIDED) { + (control_mode == GUIDED || control_mode == AVOID_ADSB)) { // don't mix in user input return false; } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 9baf75f107..212dcaa8b1 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -198,7 +198,7 @@ void Plane::update_loiter(uint16_t radius) auto_state.wp_proportion > 1) { // we've reached the target, start the timer loiter.start_time_ms = millis(); - if (control_mode == GUIDED) { + if (control_mode == GUIDED || control_mode == AVOID_ADSB) { // starting a loiter in GUIDED means we just reached the target point gcs_send_mission_item_reached_message(0); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1762d8ef24..60f39816b6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1253,7 +1253,7 @@ bool QuadPlane::in_vtol_mode(void) plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL || - (plane.control_mode == GUIDED && plane.auto_state.vtol_loiter) || + ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) || in_vtol_auto()); } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index ab3ae0988c..3c17a89ac9 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -450,6 +450,7 @@ void Plane::set_mode(enum FlightMode mode) do_loiter_at_location(); break; + case AVOID_ADSB: case GUIDED: auto_throttle_mode = true; auto_navigation_mode = true; @@ -507,6 +508,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case AUTOTUNE: case FLY_BY_WIRE_B: case CRUISE: + case AVOID_ADSB: case GUIDED: case AUTO: case RTL: @@ -706,6 +708,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case LOITER: port->print("Loiter"); break; + case AVOID_ADSB: + port->print("AVOID_ADSB"); + break; case GUIDED: port->print("Guided"); break;