From 2d796f408d1aec211cf525ed8d52125fc77b1d59 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 4 Jun 2015 03:27:40 -1000 Subject: [PATCH] Ran Astyle --- src/modules/uavcan/sensors/baro.cpp | 145 ++++++++++++++++------------ src/modules/uavcan/sensors/baro.hpp | 26 ++--- src/modules/uavcan/uavcan_main.cpp | 6 +- 3 files changed, 97 insertions(+), 80 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 658b3fb41c..ea7fa09bf7 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -40,38 +40,44 @@ 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_pressure_data(node), -_sub_air_temperature_data(node), -_reports(nullptr) +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), + _sub_air_pressure_data(node), + _sub_air_temperature_data(node), + _reports(nullptr) { - last_temperature = 0.0f; + last_temperature = 0.0f; } int UavcanBarometerBridge::init() { int res = device::CDev::init(); + if (res < 0) { return res; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - if (_reports == nullptr) + + if (_reports == nullptr) { return -1; + } - 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_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; + } - 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; } @@ -82,8 +88,9 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { if (_reports->get(baro_buf)) { @@ -100,71 +107,81 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case BAROIOCSMSLPRESSURE: { - if ((arg < 80000) || (arg > 120000)) { - return -EINVAL; - } else { - log("new msl pressure %u", _msl_pressure); - _msl_pressure = arg; + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + + } else { + 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; } - } - case BAROIOCGMSLPRESSURE: { - return _msl_pressure; - } - case SENSORIOCSPOLLRATE: { - // not supported yet, pretend that everything is ok - return OK; - } + case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); - return OK; - } default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanBarometerBridge::air_temperature_sub_cb(const + uavcan::ReceivedDataStructure &msg) { - last_temperature = msg.static_temperature; + last_temperature = msg.static_temperature; } -void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanBarometerBridge::air_pressure_sub_cb(const + uavcan::ReceivedDataStructure &msg) { - baro_report report; + 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; + 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 + /* + * 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 + 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; + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - // add to the ring buffer - _reports->force(&report); + // add to the ring buffer + _reports->force(&report); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 4c68c497fc..1303a88356 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -61,23 +61,23 @@ private: ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); - void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder < UavcanBarometerBridge *, - void (UavcanBarometerBridge::*) - (const uavcan::ReceivedDataStructure &) > - AirPressureCbBinder; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirPressureCbBinder; - typedef uavcan::MethodBinder < UavcanBarometerBridge *, - void (UavcanBarometerBridge::*) - (const uavcan::ReceivedDataStructure &) > - AirTemperatureCbBinder; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirTemperatureCbBinder; - uavcan::Subscriber _sub_air_pressure_data; - uavcan::Subscriber _sub_air_temperature_data; + uavcan::Subscriber _sub_air_pressure_data; + uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; ringbuffer::RingBuffer *_reports; - float last_temperature; + float last_temperature; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index aa3be610f4..2f3e1d57a0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -217,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { warnx("start failed: %d", errno); @@ -425,8 +425,7 @@ int UavcanNode::run() 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); _task_should_exit = true; } @@ -515,6 +514,7 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_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.noutputs = _test_motor.motor_number + 1;