Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: use `is_guided_mode` rather than pointer compare

This commit is contained in:
Iampete1 2024-12-21 20:49:37 +00:00
parent 8dc58b8e08
commit 29e9e72a46
8 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -866,7 +866,7 @@ bool Plane::set_target_location(const Location &target_loc)
{ {
Location loc{target_loc}; Location loc{target_loc};
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
// only accept position updates when in GUIDED mode // only accept position updates when in GUIDED mode
return false; return false;
} }

14
ArduPlane/GCS_Mavlink.cpp
View File

@ -868,7 +868,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
// location is valid load and set // location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == &plane.mode_guided)) { plane.control_mode->is_guided_mode()) {
plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
@ -899,7 +899,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
switch(packet.command) { switch(packet.command) {
case MAV_CMD_GUIDED_CHANGE_SPEED: { case MAV_CMD_GUIDED_CHANGE_SPEED: {
// command is only valid in guided mode // command is only valid in guided mode
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
@ -938,7 +938,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
case MAV_CMD_GUIDED_CHANGE_ALTITUDE: { case MAV_CMD_GUIDED_CHANGE_ALTITUDE: {
// command is only valid in guided // command is only valid in guided
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
@ -972,7 +972,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
case MAV_CMD_GUIDED_CHANGE_HEADING: { case MAV_CMD_GUIDED_CHANGE_HEADING: {
// command is only valid in guided mode // command is only valid in guided mode
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
@ -1306,7 +1306,7 @@ void GCS_MAVLINK_Plane::handle_set_attitude_target(const mavlink_message_t &msg)
// in e.g., RTL, CICLE. Specifying a single mode for companion // in e.g., RTL, CICLE. Specifying a single mode for companion
// computer control is more safe (even more so when using // computer control is more safe (even more so when using
// FENCE_ACTION = 4 for geofence failures). // FENCE_ACTION = 4 for geofence failures).
if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes if (!plane.control_mode->is_guided_mode()) { // don't screw up failsafes
return; return;
} }
@ -1374,7 +1374,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_messa
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// exit if vehicle is not in Guided mode // exit if vehicle is not in Guided mode
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
return; return;
} }
@ -1397,7 +1397,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess
// in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode // in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode
// for companion computer control is more safe (provided // for companion computer control is more safe (provided
// one uses the FENCE_ACTION = 4 (RTL) for geofence failures). // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
//don't screw up failsafes //don't screw up failsafes
return; return;
} }

2
ArduPlane/Log.cpp
View File

@ -268,7 +268,7 @@ void Plane::Log_Write_RC(void)
void Plane::Log_Write_Guided(void) void Plane::Log_Write_Guided(void)
{ {
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode != &mode_guided) { if (!control_mode->is_guided_mode()) {
return; return;
} }

2
ArduPlane/mode_auto.cpp
View File

@ -6,7 +6,7 @@ bool ModeAuto::_enter()
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
// check if we should refuse auto mode due to a missing takeoff in // check if we should refuse auto mode due to a missing takeoff in
// guided_wait_takeoff state // guided_wait_takeoff state
if (plane.previous_mode == &plane.mode_guided && if (plane.previous_mode->is_guided_mode() &&
quadplane.guided_wait_takeoff_on_mode_enter) { quadplane.guided_wait_takeoff_on_mode_enter) {
if (!plane.mission.starts_with_takeoff_cmd()) { if (!plane.mission.starts_with_takeoff_cmd()) {
gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required"); gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required");

2
ArduPlane/mode_guided.cpp
View File

@ -45,7 +45,7 @@ void ModeGuided::update()
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
// This function is used in Guided and AvoidADSB, check for guided // This function is used in Guided and AvoidADSB, check for guided
} else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { } else if (plane.control_mode->is_guided_mode() && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f; float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f;
plane.guided_state.target_heading_time_ms = tnow; plane.guided_state.target_heading_time_ms = tnow;

2
ArduPlane/mode_qloiter.cpp
View File

@ -151,7 +151,7 @@ void ModeQLoiter::run()
pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0); pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
quadplane.check_land_complete(); quadplane.check_land_complete();
} else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) { } else if (plane.control_mode->is_guided_mode() && quadplane.guided_takeoff) {
quadplane.set_climb_rate_cms(0); quadplane.set_climb_rate_cms(0);
} else { } else {
// update altitude target and call position controller // update altitude target and call position controller

8
ArduPlane/navigation.cpp
View File

@ -188,7 +188,7 @@ void Plane::calc_airspeed_errors()
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
} }
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped } else if (control_mode->is_guided_mode() && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped
// offboard airspeed demanded // offboard airspeed demanded
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms); float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
@ -229,7 +229,7 @@ void Plane::calc_airspeed_errors()
} else if (flight_stage == AP_FixedWing::FlightStage::LAND) { } else if (flight_stage == AP_FixedWing::FlightStage::LAND) {
// Landing airspeed target // Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm(); target_airspeed_cm = landing.get_target_airspeed_cm();
} else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert } else if (control_mode->is_guided_mode() && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
target_airspeed_cm = new_airspeed_cm; target_airspeed_cm = new_airspeed_cm;
} else if (control_mode == &mode_auto) { } else if (control_mode == &mode_auto) {
target_airspeed_cm = mode_auto_target_airspeed_cm(); target_airspeed_cm = mode_auto_target_airspeed_cm();
@ -257,7 +257,7 @@ void Plane::calc_airspeed_errors()
// when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely. // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) { if (control_mode->is_guided_mode() && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
} }
#endif #endif
@ -319,7 +319,7 @@ void Plane::update_loiter_update_nav(uint16_t radius)
#endif #endif
if ((loiter.start_time_ms == 0 && if ((loiter.start_time_ms == 0 &&
(control_mode == &mode_auto || control_mode == &mode_guided) && (control_mode == &mode_auto || control_mode->is_guided_mode()) &&
auto_state.crosstrack && auto_state.crosstrack &&
current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) || current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
quadplane_qrtl_switch) { quadplane_qrtl_switch) {

22
ArduPlane/quadplane.cpp
View File

@ -1166,7 +1166,7 @@ bool QuadPlane::is_flying(void)
if (!available()) { if (!available()) {
return false; return false;
} }
if (plane.control_mode == &plane.mode_guided && guided_takeoff) { if (plane.control_mode->is_guided_mode() && guided_takeoff) {
return true; return true;
} }
if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) { if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) {
@ -1217,7 +1217,7 @@ bool QuadPlane::is_flying_vtol(void) const
// in manual throttle modes with airmode on, don't consider aircraft landed // in manual throttle modes with airmode on, don't consider aircraft landed
return true; return true;
} }
if (plane.control_mode == &plane.mode_guided && guided_takeoff) { if (plane.control_mode->is_guided_mode() && guided_takeoff) {
return true; return true;
} }
if (plane.control_mode->is_vtol_man_mode()) { if (plane.control_mode->is_vtol_man_mode()) {
@ -1385,7 +1385,7 @@ void QuadPlane::set_armed(bool armed)
} }
motors->armed(armed); motors->armed(armed);
if (plane.control_mode == &plane.mode_guided) { if (plane.control_mode->is_guided_mode()) {
guided_wait_takeoff = armed; guided_wait_takeoff = armed;
} }
@ -2082,7 +2082,7 @@ bool QuadPlane::in_vtol_mode(void) const
poscontrol.get_state() > QPOS_APPROACH) { poscontrol.get_state() > QPOS_APPROACH) {
return true; return true;
} }
if (plane.control_mode == &plane.mode_guided && if (plane.control_mode->is_guided_mode() &&
guided_takeoff) { guided_takeoff) {
return true; return true;
} }
@ -2783,7 +2783,7 @@ void QuadPlane::vtol_position_controller(void)
break; break;
} }
} }
if (plane.control_mode == &plane.mode_guided || vtol_loiter_auto) { if (plane.control_mode->is_guided_mode() || vtol_loiter_auto) {
plane.ahrs.get_location(plane.current_loc); plane.ahrs.get_location(plane.current_loc);
int32_t target_altitude_cm; int32_t target_altitude_cm;
if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,target_altitude_cm)) { if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,target_altitude_cm)) {
@ -3066,7 +3066,7 @@ void QuadPlane::takeoff_controller(void)
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
const auto spool_state = motors->get_desired_spool_state(); const auto spool_state = motors->get_desired_spool_state();
if (plane.control_mode == &plane.mode_guided && guided_takeoff if (plane.control_mode->is_guided_mode() && guided_takeoff
&& tiltrotor.enabled() && !tiltrotor.fully_up() && && tiltrotor.enabled() && !tiltrotor.fully_up() &&
spool_state != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { spool_state != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// waiting for motors to tilt up // waiting for motors to tilt up
@ -3140,7 +3140,7 @@ void QuadPlane::takeoff_controller(void)
get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
float vel_z = wp_nav->get_default_speed_up(); float vel_z = wp_nav->get_default_speed_up();
if (plane.control_mode == &plane.mode_guided && guided_takeoff) { if (plane.control_mode->is_guided_mode() && guided_takeoff) {
// for guided takeoff we aim for a specific height with zero // for guided takeoff we aim for a specific height with zero
// velocity at that height // velocity at that height
Location origin; Location origin;
@ -3856,7 +3856,7 @@ void QuadPlane::guided_start(void)
*/ */
void QuadPlane::guided_update(void) void QuadPlane::guided_update(void)
{ {
if (plane.control_mode == &plane.mode_guided && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) { if (plane.control_mode->is_guided_mode() && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
throttle_wait = false; throttle_wait = false;
set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
takeoff_controller(); takeoff_controller();
@ -3887,7 +3887,7 @@ bool QuadPlane::guided_mode_enabled(void)
return false; return false;
} }
// only use quadplane guided when in AUTO or GUIDED mode // only use quadplane guided when in AUTO or GUIDED mode
if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) { if (!plane.control_mode->is_guided_mode() && plane.control_mode != &plane.mode_auto) {
return false; return false;
} }
if (plane.control_mode == &plane.mode_auto && if (plane.control_mode == &plane.mode_auto &&
@ -3909,7 +3909,7 @@ void QuadPlane::set_alt_target_current(void)
// user initiated takeoff for guided mode // user initiated takeoff for guided mode
bool QuadPlane::do_user_takeoff(float takeoff_altitude) bool QuadPlane::do_user_takeoff(float takeoff_altitude)
{ {
if (plane.control_mode != &plane.mode_guided) { if (!plane.control_mode->is_guided_mode()) {
gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode"); gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode");
return false; return false;
} }
@ -4745,7 +4745,7 @@ bool QuadPlane::should_disable_TECS() const
if (in_vtol_land_descent()) { if (in_vtol_land_descent()) {
return true; return true;
} }
if (plane.control_mode == &plane.mode_guided && if (plane.control_mode->is_guided_mode() &&
plane.auto_state.vtol_loiter) { plane.auto_state.vtol_loiter) {
return true; return true;
} }