fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 23:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changes to build on latest uavcan master with FW upload and Node ID allocation

Browse Source
This commit is contained in:
David Sidrane
2015-06-04 02:37:59 -10:00
parent 9ce7fe483a
commit 6155a1557f
6 changed files with 243 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/sensors/baro.cpp
+45 -32
View File
@@ -42,9 +42,11 @@ const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_data(node),
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
_reports(nullptr)
{
last_temperature = 0.0f;
}
int UavcanBarometerBridge::init()
@@ -59,11 +61,17 @@ int UavcanBarometerBridge::init()
if (_reports == nullptr)
return -1;
res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
@@ -127,31 +135,36 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{
baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
_reports->force(&report);
publish(msg.getSrcNodeID().get(), &report);
last_temperature = msg.static_temperature;
}
void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
{
baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = last_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
_reports->force(&report);
publish(msg.getSrcNodeID().get(), &report);
}