fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 14:54:07 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
src/modules/uavcan/sensors/baro.cpp
View File

@ -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<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.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);
}

26
src/modules/uavcan/sensors/baro.hpp
View File

@ -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<uavcan::equipment::air_data::StaticPressure> &msg);
void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &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);
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
AirPressureCbBinder;
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
AirPressureCbBinder;
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
AirTemperatureCbBinder;
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
AirTemperatureCbBinder;
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::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports;
float last_temperature;
float last_temperature;
};

6
src/modules/uavcan/uavcan_main.cpp
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();};
_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) {
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;