dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw_pos_control_l1: replace sensor_baro with vehicle_air_data 

- controllers shouldn't be accessing raw sensor data directly
This commit is contained in:
Daniel Agar 2020-07-12 11:08:21 -04:00
parent 1394b5d7bc
commit 9a1ca00285
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -1754,13 +1754,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* scale throttle cruise by baro pressure */
if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
sensor_baro_s baro{};
vehicle_air_data_s air_data;
if (_sensor_baro_sub.update(&baro)) {
if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
if (_vehicle_air_data_sub.update(&air_data)) {
if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure);
const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.0f, 1.0f, 2.0f);
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa);
const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f);
throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);

4
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -75,9 +75,9 @@
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -155,7 +155,7 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription