Skip to content

Commit

Permalink
ArduPlane: change namespace of MultiCopter and FixedWing params
Browse files Browse the repository at this point in the history
this stops the libraries knowing anything about AP_Vehicle
  • Loading branch information
peterbarker authored and tridge committed Nov 9, 2022
1 parent 1d19567 commit de4dda2
Show file tree
Hide file tree
Showing 17 changed files with 82 additions and 78 deletions.
35 changes: 18 additions & 17 deletions ArduPlane/ArduPlane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -479,7 +479,7 @@ void Plane::update_fly_forward(void)
}
#endif

if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
return;
}
Expand All @@ -490,15 +490,15 @@ void Plane::update_fly_forward(void)
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
void Plane::set_flight_stage(AP_FixedWing::FlightStage fs)
{
if (fs == flight_stage) {
return;
}

landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
landing.handle_flight_stage_change(fs == AP_FixedWing::FlightStage::LAND);

if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (fs == AP_FixedWing::FlightStage::ABORT_LANDING) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
int(auto_state.takeoff_altitude_rel_cm/100));
}
Expand Down Expand Up @@ -546,7 +546,8 @@ void Plane::update_alt()
if (control_mode->does_auto_throttle() && !throttle_suppressed) {

float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
if (flight_stage == AP_FixedWing::FlightStage::LAND &&
current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
}

Expand Down Expand Up @@ -580,49 +581,49 @@ void Plane::update_flight_stage(void)
if (control_mode == &mode_auto) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
set_flight_stage(AP_FixedWing::FlightStage::VTOL);
return;
}
#endif
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
return;
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
} else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
landing.request_go_around()) {
gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
set_flight_stage(AP_FixedWing::FlightStage::LAND);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
set_flight_stage(AP_FixedWing::FlightStage::VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
} else if (control_mode != &mode_takeoff) {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
set_flight_stage(AP_FixedWing::FlightStage::VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}


Expand Down Expand Up @@ -662,7 +663,7 @@ float Plane::tecs_hgt_afe(void)
coming.
*/
float hgt_afe;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();
} else {
Expand Down
16 changes: 8 additions & 8 deletions ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ float Plane::calc_speed_scaler(void)
}
if (!plane.ahrs.airspeed_sensor_enabled() &&
(plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) &&
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates
(plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates
return MIN(speed_scaler, 1.0f) ;
}
return speed_scaler;
Expand Down Expand Up @@ -767,8 +767,8 @@ void Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
is_zero(get_throttle_input()) &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
flight_stage != AP_FixedWing::FlightStage::TAKEOFF &&
flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
// manual rudder control while still
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
Expand All @@ -784,8 +784,8 @@ void Plane::calc_nav_yaw_ground(void)
steer_state.last_steer_ms = now_ms;

float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF ||
flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
steer_rate = 0;
}
if (!is_zero(steer_rate)) {
Expand All @@ -794,8 +794,8 @@ void Plane::calc_nav_yaw_ground(void)
} else if (!steer_state.locked_course) {
// pilot has released the rudder stick or we are still - lock the course
steer_state.locked_course = true;
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (flight_stage != AP_FixedWing::FlightStage::TAKEOFF &&
flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
steer_state.locked_course_err = 0;
}
}
Expand Down Expand Up @@ -889,7 +889,7 @@ void Plane::calc_nav_roll()
void Plane::adjust_nav_pitch_throttle(void)
{
int8_t throttle = throttle_percentage();
if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_Vehicle::FixedWing::FLIGHT_VTOL) {
if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_FixedWing::FlightStage::VTOL) {
float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
}
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -357,7 +357,7 @@ void GCS_MAVLINK_Plane::send_pid_tuning()
pid_info = &plane.steerController.get_pid_info();
send_pid_info(pid_info, PID_TUNING_STEER, pid_info->actual);
}
if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_FixedWing::FlightStage::LAND)) {
AP_AHRS &ahrs = AP::ahrs();
const Vector3f &gyro = ahrs.get_gyro();
send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
Expand Down Expand Up @@ -1022,7 +1022,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
case MAV_CMD_DO_GO_AROUND:
{
uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
bool is_in_landing = (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) ||
bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
(mission_id == MAV_CMD_NAV_LAND) ||
(mission_id == MAV_CMD_NAV_VTOL_LAND);
if (is_in_landing) {
Expand Down
10 changes: 5 additions & 5 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ class Plane : public AP_Vehicle {
private:

// key aircraft parameters passed to multiple libraries
AP_Vehicle::FixedWing aparm;
AP_FixedWing aparm;

// Global parameters are all contained within the 'g' and 'g2' classes.
Parameters g;
Expand All @@ -191,7 +191,7 @@ class Plane : public AP_Vehicle {
// flight modes convenience array
AP_Int8 *flight_modes = &g.flight_mode1;

AP_Vehicle::FixedWing::Rangefinder_State rangefinder_state;
AP_FixedWing::Rangefinder_State rangefinder_state;

#if AP_RPM_ENABLED
AP_RPM rpm_sensor;
Expand Down Expand Up @@ -561,7 +561,7 @@ class Plane : public AP_Vehicle {
#if LANDING_GEAR_ENABLED == ENABLED
// landing gear state
struct {
AP_Vehicle::FixedWing::FlightStage last_flight_stage;
AP_FixedWing::FlightStage last_flight_stage;
} gear;
#endif

Expand Down Expand Up @@ -594,7 +594,7 @@ class Plane : public AP_Vehicle {
int8_t throttle_watt_limit_min; // for reverse thrust
uint32_t throttle_watt_limit_timer_ms;

AP_Vehicle::FixedWing::FlightStage flight_stage = AP_Vehicle::FixedWing::FLIGHT_NORMAL;
AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL;

// probability of aircraft is currently in flight. range from 0 to
// 1 where 1 is 100% sure we're in flight
Expand Down Expand Up @@ -1013,7 +1013,7 @@ class Plane : public AP_Vehicle {
void update_control_mode(void);
void update_fly_forward(void);
void update_flight_stage();
void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs);
void set_flight_stage(AP_FixedWing::FlightStage fs);

// navigation.cpp
void loiter_angle_reset(void);
Expand Down
10 changes: 5 additions & 5 deletions ArduPlane/altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -448,7 +448,7 @@ void Plane::set_offset_altitude_location(const Location &start_loc, const Locati
}
#endif

if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage != AP_FixedWing::FlightStage::LAND) {
// if we are within GLIDE_SLOPE_MIN meters of the target altitude
// then reset the offset to not use a glide slope. This allows for
// more accurate flight of missions where the aircraft may lose or
Expand Down Expand Up @@ -539,7 +539,7 @@ float Plane::mission_alt_offset(void)
{
float ret = g.alt_offset;
if (control_mode == &mode_auto &&
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) {
(flight_stage == AP_FixedWing::FlightStage::LAND || auto_state.wp_is_land_approach)) {
// when landing after an aborted landing due to too high glide
// slope we use an offset from the last landing attempt
ret += landing.alt_offset;
Expand Down Expand Up @@ -641,7 +641,7 @@ float Plane::rangefinder_correction(void)
}

// for now we only support the rangefinder for landing
bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_FixedWing::FlightStage::LAND);
if (!using_rangefinder) {
return 0;
}
Expand All @@ -657,7 +657,7 @@ void Plane::rangefinder_terrain_correction(float &height)
{
#if AP_TERRAIN_AVAILABLE
if (!g.rangefinder_landing ||
flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND ||
flight_stage != AP_FixedWing::FlightStage::LAND ||
!terrain_enabled_in_current_mode()) {
return;
}
Expand Down Expand Up @@ -707,7 +707,7 @@ void Plane::rangefinder_height_update(void)
} else {
rangefinder_state.in_range = true;
bool flightstage_good_for_rangefinder_landing = false;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
flightstage_good_for_rangefinder_landing = true;
}
#if HAL_QUADPLANE_ENABLED
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/avoidance_adsb.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
bool flightmode_prohibits_action = false;
if (plane.control_mode == &plane.mode_manual ||
(plane.control_mode == &plane.mode_auto && !plane.auto_state.takeoff_complete) ||
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || // TODO: consider allowing action during approach
(plane.flight_stage == AP_FixedWing::FlightStage::LAND) || // TODO: consider allowing action during approach
plane.control_mode == &plane.mode_autotune) {
flightmode_prohibits_action = true;
}
Expand Down
6 changes: 3 additions & 3 deletions ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -246,7 +246,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
return quadplane.verify_vtol_land();
}
#endif
if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);

} else {
Expand Down Expand Up @@ -413,9 +413,9 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)

landing.do_land(cmd, relative_altitude);

if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
// if we were in an abort we need to explicitly move out of the abort state, as it's sticky
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
set_flight_stage(AP_FixedWing::FlightStage::LAND);
}

#if AP_FENCE_ENABLED
Expand Down
6 changes: 3 additions & 3 deletions ArduPlane/events.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
// for use in failsafe code.
bool Plane::failsafe_in_landing_sequence() const
{
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
return true;
}
#if HAL_QUADPLANE_ENABLED
Expand Down Expand Up @@ -238,7 +238,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
FALLTHROUGH;
#endif // HAL_QUADPLANE_ENABLED
case Failsafe_Action_Land: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) {
already_landing = true;
Expand All @@ -259,7 +259,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
FALLTHROUGH;
}
case Failsafe_Action_RTL: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland ||
quadplane.in_vtol_land_sequence()) {
Expand Down
Loading

0 comments on commit de4dda2

Please sign in to comment.