Plane: rename OFFBOARD_GUIDED to AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED

and remove comparison vs ENABLED
This commit is contained in:
Peter Barker 2024-08-01 13:07:38 +10:00 committed by Peter Barker
parent da562369b5
commit c2f5c48fe5
9 changed files with 22 additions and 22 deletions

View File

@ -878,8 +878,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet)
{
switch(packet.command) {
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
case MAV_CMD_GUIDED_CHANGE_SPEED: {
// command is only valid in guided mode
if (plane.control_mode != &plane.mode_guided) {
@ -1008,7 +1008,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
plane.guided_state.target_heading_time_ms = AP_HAL::millis();
return MAV_RESULT_ACCEPTED;
}
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
}

View File

@ -121,7 +121,7 @@ void Plane::Log_Write_Control_Tuning()
logger.WriteBlock(&pkt, sizeof(pkt));
}
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
struct PACKED log_OFG_Guided {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -265,7 +265,7 @@ void Plane::Log_Write_RC(void)
void Plane::Log_Write_Guided(void)
{
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode != &mode_guided) {
return;
}
@ -277,7 +277,7 @@ void Plane::Log_Write_Guided(void)
if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) {
Log_Write_OFG_Guided();
}
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
}
// incoming-to-vehicle mavlink COMMAND_INT can be logged
@ -483,7 +483,7 @@ const struct LogStructure Plane::log_structure[] = {
{ LOG_AETR_MSG, sizeof(log_AETR),
"AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true },
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @LoggerMessage: OFG
// @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s.
// @Field: TimeUS: Time since system startup

View File

@ -1218,11 +1218,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @Group: GUIDED_
// @Path: ../libraries/AC_PID/AC_PID.cpp
AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID),
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @Param: MAN_EXPO_ROLL
// @DisplayName: Manual control expo for roll

View File

@ -555,7 +555,7 @@ public:
} fwd_batt_cmp;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// guided yaw heading PID
AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0};
#endif

View File

@ -553,7 +553,7 @@ private:
float forced_throttle;
uint32_t last_forced_throttle_ms;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// airspeed adjustments
float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed.
float target_airspeed_accel;
@ -572,7 +572,7 @@ private:
uint32_t target_heading_time_ms;
guided_heading_type_t target_heading_type;
bool target_heading_limit;
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} guided_state;
#if AP_LANDINGGEAR_ENABLED

View File

@ -213,8 +213,8 @@
# define FENCE_TRIGGERED_PIN -1
#endif
#ifndef OFFBOARD_GUIDED
#define OFFBOARD_GUIDED 1
#ifndef AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
#define AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED 1
#endif
//////////////////////////////////////////////////////////////////////////////

View File

@ -54,7 +54,7 @@ bool Mode::enter()
plane.guided_state.last_forced_rpy_ms.zero();
plane.guided_state.last_forced_throttle_ms = 0;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.

View File

@ -42,7 +42,7 @@ void ModeGuided::update()
plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
plane.update_load_factor();
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// 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
} else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
@ -70,7 +70,7 @@ void ModeGuided::update()
plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
plane.update_load_factor();
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} else {
plane.calc_nav_roll();
}
@ -128,7 +128,7 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi
void ModeGuided::update_target_altitude()
{
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
// offboard altitude demanded
uint32_t now = AP_HAL::millis();
@ -148,7 +148,7 @@ void ModeGuided::update_target_altitude()
plane.guided_state.last_target_alt = temp.alt;
plane.set_target_altitude_location(temp);
} else
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
{
Mode::update_target_altitude();
}

View File

@ -187,7 +187,7 @@ void Plane::calc_airspeed_errors()
target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
}
#if OFFBOARD_GUIDED == 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
// offboard airspeed demanded
uint32_t now = AP_HAL::millis();
@ -203,7 +203,7 @@ void Plane::calc_airspeed_errors()
target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
}
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
#if HAL_SOARING_ENABLED
} else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
@ -256,7 +256,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.
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
}