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

Ran Astyle

Browse Source
This commit is contained in:
David Sidrane
2015-06-04 03:27:40 -10:00
parent 3f56892950
commit 2d796f408d
3 changed files with 97 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/sensors/baro.cpp
+81 -64
View File
@@ -40,38 +40,44 @@
const char *const UavcanBarometerBridge::NAME = "baro"; const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_pressure_data(node), _sub_air_pressure_data(node),
_sub_air_temperature_data(node), _sub_air_temperature_data(node),
_reports(nullptr) _reports(nullptr)
{ {
last_temperature = 0.0f; last_temperature = 0.0f;
} }
int UavcanBarometerBridge::init() int UavcanBarometerBridge::init()
{ {
int res = device::CDev::init(); int res = device::CDev::init();
if (res < 0) { if (res < 0) {
return res; return res;
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr)
if (_reports == nullptr) {
return -1; return -1;
}
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res); if (res < 0) {
return res; 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;
}
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; return 0;
} }
@@ -82,8 +88,9 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
if (count < 1) if (count < 1) {
return -ENOSPC; return -ENOSPC;
}
while (count--) { while (count--) {
if (_reports->get(baro_buf)) { if (_reports->get(baro_buf)) {
@@ -100,71 +107,81 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{ {
switch (cmd) { switch (cmd) {
case BAROIOCSMSLPRESSURE: { case BAROIOCSMSLPRESSURE: {
if ((arg < 80000) || (arg > 120000)) { if ((arg < 80000) || (arg > 120000)) {
return -EINVAL; return -EINVAL;
} else {
log("new msl pressure %u", _msl_pressure); } else {
_msl_pressure = arg; log("new msl pressure %u", _msl_pressure);
_msl_pressure = arg;
return OK;
}
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
case SENSORIOCSPOLLRATE: {
// not supported yet, pretend that everything is ok
return OK; return OK;
} }
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
case SENSORIOCSPOLLRATE: {
// not supported yet, pretend that everything is ok
return OK;
}
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags); irqrestore(flags);
return -ENOMEM;
return OK;
} }
irqrestore(flags);
return OK;
}
default: { default: {
return CDev::ioctl(filp, cmd, arg); return CDev::ioctl(filp, cmd, arg);
} }
} }
} }
void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg) void UavcanBarometerBridge::air_temperature_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{ {
last_temperature = msg.static_temperature; last_temperature = msg.static_temperature;
} }
void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg) void UavcanBarometerBridge::air_pressure_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
{ {
baro_report report; baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = last_temperature; report.temperature = last_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0; report.error_count = 0;
/* /*
* Altitude computation * Altitude computation
* Refer to the MS5611 driver for details * Refer to the MS5611 driver for details
*/ */
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin 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 a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s 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 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 p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure 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; report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer // add to the ring buffer
_reports->force(&report); _reports->force(&report);
publish(msg.getSrcNodeID().get(), &report); publish(msg.getSrcNodeID().get(), &report);
} }
src/modules/uavcan/sensors/baro.hpp
+13 -13
View File
@@ -61,23 +61,23 @@ private:
ssize_t read(struct file *filp, char *buffer, size_t buflen); ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override; int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg); void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg); void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
typedef uavcan::MethodBinder < UavcanBarometerBridge *, typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*) void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) > (const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
AirPressureCbBinder; AirPressureCbBinder;
typedef uavcan::MethodBinder < UavcanBarometerBridge *, typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*) void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) > (const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
AirTemperatureCbBinder; AirTemperatureCbBinder;
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data; uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data; uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
unsigned _msl_pressure = 101325; unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports; ringbuffer::RingBuffer *_reports;
float last_temperature; float last_temperature;
}; };
src/modules/uavcan/uavcan_main.cpp
+3 -3
View File
@@ -217,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
*/ */
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr); static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) { if (_instance->_task < 0) {
warnx("start failed: %d", errno); warnx("start failed: %d", errno);
@@ -425,8 +425,7 @@ int UavcanNode::run()
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) if (busevent_fd < 0) {
{
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true; _task_should_exit = true;
} }
@@ -515,6 +514,7 @@ int UavcanNode::run()
// can we mix? // can we mix?
if (_test_in_progress) { if (_test_in_progress) {
memset(&_outputs, 0, sizeof(_outputs)); memset(&_outputs, 0, sizeof(_outputs));
if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
_outputs.noutputs = _test_motor.motor_number + 1; _outputs.noutputs = _test_motor.motor_number + 1;