From 3576d513cd15bfddb96e7a9a2f2dcc6eb941b4df Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:32:09 +0100 Subject: [PATCH] =?UTF-8?q?battery:=20make=20time=20remaining=20estimation?= =?UTF-8?q?=20dependent=20on=20level=20flight=20cha=E2=80=A6=20(#22401)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * battery: make time remaining estimation dependent on level flight characteristis for FW * battery: fix that FW flight is also correctly detected when vehicle_status is not updated Signed-off-by: Silvan Fuhrer * FixedwingPositionControl: Move constant to header file * flight phase estimation: use tecs height rate reference to check for level flight --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Silvan Fuhrer --- msg/BatteryStatus.msg | 2 +- msg/CMakeLists.txt | 1 + msg/FlightPhaseEstimation.msg | 8 +++++ src/lib/battery/battery.cpp | 11 +++++-- src/lib/battery/battery.h | 6 +++- .../FixedwingPositionControl.cpp | 29 +++++++++++++++++++ .../FixedwingPositionControl.hpp | 6 ++++ src/modules/logger/logged_topics.cpp | 1 + 8 files changed, 60 insertions(+), 4 deletions(-) create mode 100644 msg/FlightPhaseEstimation.msg diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 65845b57ae..66fcffa0e6 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -4,7 +4,7 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown -float32 current_average_a # Battery current average in amperes, -1 if unknown +float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 58b08afa4b..eaad5510bb 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -93,6 +93,7 @@ set(msg_files FigureEightStatus.msg FailsafeFlags.msg FailureDetectorStatus.msg + FlightPhaseEstimation.msg FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg diff --git a/msg/FlightPhaseEstimation.msg b/msg/FlightPhaseEstimation.msg new file mode 100644 index 0000000000..e05b329129 --- /dev/null +++ b/msg/FlightPhaseEstimation.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 flight_phase # Estimate of current flight phase + +uint8 FLIGHT_PHASE_UNKNOWN = 0 # vehicle flight phase is unknown +uint8 FLIGHT_PHASE_LEVEL = 1 # Vehicle is in level flight +uint8 FLIGHT_PHASE_DESCEND = 2 # vehicle is in descend +uint8 FLIGHT_PHASE_CLIMB = 3 # vehicle is climbing diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 4408754d1a..8c833af1e0 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -311,16 +311,23 @@ float Battery::computeRemainingTime(float current_a) if (_vehicle_status_sub.copy(&vehicle_status)) { _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); } } + _flight_phase_estimation_sub.update(); + if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) { _current_average_filter_a.reset(_params.bat_avrg_current); } if (_armed && PX4_ISFINITE(current_a)) { - // only update with positive numbers - _current_average_filter_a.update(fmaxf(current_a, 0.f)); + // For FW only update when we are in level flight + if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s + && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) { + // only update with positive numbers + _current_average_filter_a.update(fmaxf(current_a, 0.f)); + } } // Remaining time estimation only possible with capacity diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 66c923071e..91edbe0580 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -156,6 +157,7 @@ private: uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; bool _external_state_of_charge{false}; ///< inticates that the soc is injected and not updated by this library @@ -168,7 +170,8 @@ private: AlphaFilter _voltage_filter_v; float _current_a{-1}; AlphaFilter _current_filter_a; - AlphaFilter _current_average_filter_a; + AlphaFilter + _current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight. AlphaFilter _throttle_filter; float _discharged_mah{0.f}; float _discharged_mah_loop{0.f}; @@ -178,5 +181,6 @@ private: uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _last_timestamp{0}; bool _armed{false}; + bool _vehicle_status_is_fw{false}; hrt_abstime _last_unconnected_timestamp{0}; }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8ffb0157f2..5672e7f446 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2454,6 +2454,9 @@ FixedwingPositionControl::Run() _flaps_setpoint = 0.f; _spoilers_setpoint = 0.f; + // reset flight phase estimate + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; + // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw_wheel = false; @@ -2569,6 +2572,10 @@ FixedwingPositionControl::Run() _roll_slew_rate.setForcedValue(_roll); } + // Publish estimate of level flight + _flight_phase_estimation_pub.get().timestamp = hrt_absolute_time(); + _flight_phase_estimation_pub.update(); + // if there's any change in landing gear setpoint publish it if (_new_landing_gear_position != old_landing_gear_position && _new_landing_gear_position != landing_gear_s::GEAR_KEEP) { @@ -2672,6 +2679,28 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva hgt_rate_sp); tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); + + if (_tecs_is_running && !_vehicle_status.in_transition_mode + && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + const TECS::DebugOutput &tecs_output{_tecs.getStatus()}; + + // Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving + if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) && + fabsf(_current_altitude - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; + + } else if (((tecs_output.altitude_reference - _current_altitude) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; + + } else if (((_current_altitude - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; + + } else { + //We can't infer the flight phase , do nothing, estimation is reset at each step + } + } } float diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 3c5576f09d..e1593916bc 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -165,6 +166,9 @@ static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f; // [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare) static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; +// [m/s] maximum reference altitude rate threshhold +static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -215,6 +219,7 @@ private: uORB::Publication _landing_gear_pub {ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; + uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; @@ -388,6 +393,7 @@ private: TECS _tecs; bool _tecs_is_running{false}; + // VTOL / TRANSITION bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8d2406c2cf..c138da8069 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -68,6 +68,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("follow_target_estimator", 200); add_optional_topic("follow_target_status", 400); add_optional_topic("flaps_setpoint", 1000); + add_optional_topic("flight_phase_estimation", 1000); add_topic("gimbal_manager_set_attitude", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump");