yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move baro aggregation to new sensors/vehicle_air_data

Browse Source
This commit is contained in:
Daniel Agar
2020-01-20 23:28:30 -05:00
parent 093e9ba1ce
commit 0eca583ecf
25 changed files with 479 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/modules/sensors/sensors.cpp
View File

@@ -74,6 +74,7 @@
#include "voted_sensors_update.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
using namespace sensors;
@@ -112,7 +113,6 @@ private:
bool _armed{false}; /**< arming status of the vehicle */
hrt_abstime _last_config_update{0};
hrt_abstime _airdata_prev_timestamp{0};
hrt_abstime _sensor_combined_prev_timestamp{0};
hrt_abstime _magnetometer_prev_timestamp{0};
@@ -123,11 +123,11 @@ private:
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
uORB::Publication<sensor_preflight_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_air_data_s> _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] {
@@ -159,6 +159,7 @@ private:
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData _vehicle_air_data;
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -175,7 +176,7 @@ private:
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void diff_pres_poll(const vehicle_air_data_s &airdata);
void diff_pres_poll();
/**
* Check for changes in parameters.
@@ -213,6 +214,7 @@ Sensors::Sensors(bool hil_enabled) :
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
_vehicle_air_data.Start();
InitializeVehicleIMU();
}
@@ -221,6 +223,7 @@ Sensors::~Sensors()
{
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
_vehicle_air_data.Stop();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
@@ -268,15 +271,17 @@ int Sensors::adc_init()
return OK;
}
void
Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
void Sensors::diff_pres_poll()
{
differential_pressure_s diff_pres{};
if (_diff_pres_sub.update(&diff_pres)) {
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
(air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
airspeed_s airspeed{};
airspeed.timestamp = diff_pres.timestamp;
@@ -312,10 +317,10 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here
airspeed.air_temperature_celsius = air_temperature_celsius;
@@ -482,14 +487,13 @@ void Sensors::Run()
}
}
vehicle_air_data_s airdata{};
vehicle_magnetometer_s magnetometer{};
_voted_sensors_update.sensorsPoll(_sensor_combined, airdata, magnetometer);
_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
// check analog airspeed
adc_poll();
diff_pres_poll(airdata);
diff_pres_poll();
// check failover and update subscribed sensor (if necessary)
@@ -517,11 +521,6 @@ void Sensors::Run()
}
}
if ((airdata.timestamp != 0) && (airdata.timestamp != _airdata_prev_timestamp)) {
_airdata_pub.publish(airdata);
_airdata_prev_timestamp = airdata.timestamp;
}
if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
_magnetometer_pub.publish(magnetometer);
_magnetometer_prev_timestamp = magnetometer.timestamp;
@@ -618,6 +617,8 @@ int Sensors::print_status()
{
_voted_sensors_update.printStatus();
PX4_INFO_RAW("\n");
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
@@ -627,6 +628,11 @@ int Sensors::print_status()
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
PX4_INFO_RAW("\n");
_vehicle_air_data.PrintStatus();
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
PX4_INFO_RAW("\n");