fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 05:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

battery: make time remaining estimation dependent on level flight cha… (#22401)

Browse Source 
* 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 <silvan@auterion.com>

* FixedwingPositionControl: Move constant to header file

* flight phase estimation: use tecs height rate reference to check for level flight

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
KonradRudin
2024-02-06 17:32:09 +01:00
committed by GitHub
parent bf52d8adc9
commit 3576d513cd
8 changed files with 60 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+29
View File
@@ -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