Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add new flight mode AVOID_ADSB to mimic GUIDED

Browse Source
This commit is contained in:
Tom Pittenger 2016-08-12 13:27:48 -07:00
parent c4460a285c
commit ca32bcc58d
10 changed files with 27 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -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;

11
ArduPlane/Attitude.cpp
View File

@ -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;

8
ArduPlane/GCS_Mavlink.cpp
View File

@ -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;
}

1
ArduPlane/altitude.cpp
View File

@ -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

1
ArduPlane/defines.h
View File

@ -58,6 +58,7 @@ enum FlightMode {
AUTO = 10,
RTL = 11,
LOITER = 12,
AVOID_ADSB = 14,
GUIDED = 15,
INITIALISING = 16,
QSTABILIZE = 17,

2
ArduPlane/events.cpp
View File

@ -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) {

6
ArduPlane/geofence.cpp
View File

@ -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;
}

2
ArduPlane/navigation.cpp
View File

@ -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);
}

2
ArduPlane/quadplane.cpp
View File

@ -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());
}

5
ArduPlane/system.cpp
View File

@ -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;