From c1abcfb7c416971af08c8d4103c6b5452c30b139 Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Sun, 2 Jan 2022 02:41:27 -0500 Subject: [PATCH] Plane: move Airspeed to AP_Vehicle --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Parameters.cpp | 12 +++++++++--- ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 5 ----- ArduPlane/navigation.cpp | 15 ++++++++++++--- ArduPlane/sensors.cpp | 25 ------------------------- ArduPlane/system.cpp | 15 ++++----------- 7 files changed, 27 insertions(+), 49 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0dd3ba594e..13558fa2a8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -65,7 +65,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(update_GPS_10Hz, 10, 400, 33), SCHED_TASK(navigate, 10, 150, 36), SCHED_TASK(update_compass, 10, 200, 39), - SCHED_TASK(read_airspeed, 10, 100, 42), + SCHED_TASK(calc_airspeed_errors, 10, 100, 42), SCHED_TASK(update_alt, 10, 200, 45), SCHED_TASK(adjust_altitude_target, 10, 200, 48), #if ADVANCED_FAILSAFE == ENABLED diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b30a0d9db3..bd3d6cf556 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -890,9 +890,7 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), - // @Group: ARSPD - // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp - GOBJECT(airspeed, "ARSPD", AP_Airspeed), + // Airspeed was here // @Group: NAVL1_ // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp @@ -1491,6 +1489,14 @@ void Plane::load_parameters(void) } #endif + // PARAMETER_CONVERSION - Added: Jan-2022 + { + const uint16_t old_key = g.k_param_airspeed; + const uint16_t old_index = 0; // Old parameter index in the tree + const uint16_t old_top_element = 0; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, old_top_element, true); + } + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index bd770d3c5f..d22e435a47 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -196,7 +196,7 @@ public: k_param_sonar_enabled_old, // unused k_param_ahrs, // AHRS group k_param_barometer, // barometer ground calibration - k_param_airspeed, // AP_Airspeed parameters + k_param_airspeed, // only used for parameter conversion; AP_Airspeed parameters moved to AP_Vehicle k_param_curr_amp_offset, k_param_NavEKF, // deprecated - remove k_param_mission, // mission library diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1618fbb5c4..72b61d5831 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -41,7 +41,6 @@ #include // Range finder library #include // Filter library #include // Photo or video camera -#include #include #include #include // statistics library @@ -400,9 +399,6 @@ private: FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t), _failsafe_priorities}; - // Airspeed Sensors - AP_Airspeed airspeed; - // ACRO controller state struct { bool locked_roll; @@ -1047,7 +1043,6 @@ private: // sensors.cpp void read_rangefinder(void); - void read_airspeed(void); // system.cpp void init_ardupilot() override; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index a3e968487f..895fa703cf 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -140,11 +140,20 @@ float Plane::mode_auto_target_airspeed_cm() void Plane::calc_airspeed_errors() { + // Get the airspeed_estimate, update smoothed airspeed estimate + // NOTE: we use the airspeed estimate function not direct sensor + // as TECS may be using synthetic airspeed float airspeed_measured = 0; + if (ahrs.airspeed_estimate(airspeed_measured)) { + smoothed_airspeed = smoothed_airspeed * 0.8f + airspeed_measured * 0.2f; + } + + // low pass filter speed scaler, with 1Hz cutoff, at 10Hz + const float speed_scaler = calc_speed_scaler(); + const float cutoff_Hz = 2.0; + const float dt = 0.1; + surface_speed_scaler += calc_lowpass_alpha_dt(dt, cutoff_Hz) * (speed_scaler - surface_speed_scaler); - // we use the airspeed estimate function not direct sensor as TECS - // may be using synthetic airspeed - ahrs.airspeed_estimate(airspeed_measured); // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index b2e5b9817a..f89ddbeae0 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -33,28 +33,3 @@ void Plane::read_rangefinder(void) rangefinder_height_update(); } - -/* - ask airspeed sensor for a new value - */ -void Plane::read_airspeed(void) -{ - airspeed.update(should_log(MASK_LOG_IMU)); - - // we calculate airspeed errors (and thus target_airspeed_cm) even - // when airspeed is disabled as TECS may be using synthetic - // airspeed for a quadplane transition - calc_airspeed_errors(); - - // update smoothed airspeed estimate - float aspeed; - if (ahrs.airspeed_estimate(aspeed)) { - smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f; - } - - // low pass filter speed scaler, with 1Hz cutoff, at 10Hz - const float speed_scaler = calc_speed_scaler(); - const float cutoff_Hz = 2.0; - const float dt = 0.1; - surface_speed_scaler += calc_lowpass_alpha_dt(dt, cutoff_Hz) * (speed_scaler - surface_speed_scaler); -} diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index fc76eea9bb..205dda4445 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -80,12 +80,13 @@ void Plane::init_ardupilot() log_init(); #endif - // initialise airspeed sensor - airspeed.init(); - AP::compass().set_log_bit(MASK_LOG_COMPASS); AP::compass().init(); +#if AP_AIRSPEED_ENABLED + airspeed.set_log_bit(MASK_LOG_IMU); +#endif + // GPS Initialization gps.set_log_gps_bit(MASK_LOG_GPS); gps.init(serial_manager); @@ -407,14 +408,6 @@ void Plane::startup_INS_ground(void) //----------------------------- barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); - - if (airspeed.enabled()) { - // initialize airspeed sensor - // -------------------------- - airspeed.calibrate(true); - } else { - gcs().send_text(MAV_SEVERITY_WARNING,"No airspeed sensor present"); - } } // sets notify object flight mode information