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

Merge remote-tracking branch 'upstream/master' into obcfailsafe

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-24 16:26:42 +02:00
parent bdccd69030 3454c42596
commit e52f7770be
25 changed files with 964 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.uavcan
+18
View File
@@ -0,0 +1,18 @@
#!nsh
#
# UAVCAN initialization script.
#
if param compare UAVCAN_ENABLE 1
then
if uavcan start
then
# First sensor publisher to initialize takes lowest instance ID
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
sleep 1
echo "[init] UAVCAN started"
else
echo "[init] ERROR: Could not start UAVCAN"
tone_alarm $TUNE_OUT_ERROR
fi
fi
ROMFS/px4fmu_common/init.d/rcS
+8 -4
View File
@@ -304,11 +304,10 @@ then
then
if [ $OUTPUT_MODE == uavcan_esc ]
then
if uavcan start 1
if param compare UAVCAN_ENABLE 0
then
echo "CAN UP"
else
echo "CAN ERR"
echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
param set UAVCAN_ENABLE 1
fi
fi
@@ -447,6 +446,11 @@ then
mavlink start $MAVLINK_FLAGS
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Sensors, Logging, GPS
#
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: 4d7487c2bc...2423e47b4f
src/drivers/ets_airspeed/ets_airspeed.cpp
+6 -14
View File
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
uint16_t diff_pres_pa;
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
return -1;
}
if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
if (diff_pres_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = (float)diff_pres_pa;
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -369,7 +361,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */
src/drivers/meas_airspeed/meas_airspeed.cpp
+8 -29
View File
@@ -228,44 +228,23 @@ MEASAirspeed::collect()
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
float diff_press_pa = fabsf(diff_press_pa_raw);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
@@ -523,7 +502,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -551,7 +530,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
src/modules/commander/airspeed_calibration.cpp
+10 -8
View File
@@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
@@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
src/modules/commander/state_machine_helper.cpp
+13 -6
View File
@@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
}
src/modules/mavlink/mavlink_messages.cpp
+9 -2
View File
@@ -774,6 +774,9 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_combined_sub;
uint64_t _sensor_combined_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -789,7 +792,9 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0)
_airspeed_time(0),
_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_combined_time(0)
{}
void send(const hrt_abstime t)
@@ -799,12 +804,14 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@@ -813,7 +820,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = pos.alt;
msg.alt = sensor_combined.baro_alt_meter;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
src/modules/sensors/sensors.cpp
+7 -7
View File
@@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
/* don't risk to feed negative airspeed into the system */
_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
@@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
/* announce the airspeed if needed, just publish else */
src/modules/uORB/topics/differential_pressure.h
-1
View File
@@ -54,7 +54,6 @@
struct differential_pressure_s {
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
src/modules/uavcan/esc_controller.cpp → src/modules/uavcan/actuators/esc.cpp
+2 -2
View File
@@ -32,12 +32,12 @@
****************************************************************************/
/**
* @file esc_controller.cpp
* @file esc.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include "esc.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :
src/modules/uavcan/esc_controller.hpp → src/modules/uavcan/actuators/esc.hpp
+1 -1
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file esc_controller.hpp
* @file esc.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand
src/modules/uavcan/module.mk
+13 -4
View File
@@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
# Main
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
uavcan_params.c
# Actuators
SRCS += actuators/esc.cpp
# Sensors
SRCS += sensors/sensor_bridge.cpp \
sensors/gnss.cpp \
sensors/mag.cpp \
sensors/baro.cpp
#
# libuavcan
src/modules/uavcan/sensors/baro.cpp
+117
View File
@@ -0,0 +1,117 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "baro.hpp"
#include <cmath>
static const orb_id_t BARO_TOPICS[2] = {
ORB_ID(sensor_baro0),
ORB_ID(sensor_baro1)
};
const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
_sub_air_data(node)
{
}
int UavcanBarometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
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;
}
return 0;
}
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;
return OK;
}
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
{
auto report = ::baro_report();
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
report.timestamp = msg.getMonotonicTimestamp().toUSec();
}
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
/*
* 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;
publish(msg.getSrcNodeID().get(), &report);
}
src/modules/uavcan/sensors/baro.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_baro.h>
#include <uavcan/equipment/air_data/StaticAirData.hpp>
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanBarometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
typedef uavcan::MethodBinder<UavcanBarometerBridge*,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
AirDataCbBinder;
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
unsigned _msl_pressure = 101325;
};
src/modules/uavcan/gnss_receiver.cpp → src/modules/uavcan/sensors/gnss.cpp
+52 -40
View File
@@ -32,52 +32,64 @@
****************************************************************************/
/**
* @file gnss_receiver.cpp
* @file gnss.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include "gnss.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_sub_fix(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
int UavcanGnssBridge::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
return res;
}
// Clear the uORB GPS report
memset(&_report, 0, sizeof(_report));
return res;
}
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
unsigned UavcanGnssBridge::get_num_redundant_channels() const
{
_report.timestamp_position = hrt_absolute_time();
_report.lat = msg.lat_1e7;
_report.lon = msg.lon_1e7;
_report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
return (_receiver_node_id < 0) ? 0 : 1;
}
_report.timestamp_variance = _report.timestamp_position;
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
}
}
auto report = ::vehicle_gps_position_s();
report.timestamp_position = hrt_absolute_time();
report.lat = msg.lat_1e7;
report.lon = msg.lon_1e7;
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
report.timestamp_variance = report.timestamp_position;
// Check if the msg contains valid covariance information
@@ -90,19 +102,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
_report.eph = -1.0F;
_report.epv = -1.0F;
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
@@ -118,36 +130,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
_report.c_variance_rad =
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
_report.s_variance_m_s = -1.0F;
_report.c_variance_rad = -1.0F;
report.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
}
_report.fix_type = msg.status;
report.fix_type = msg.status;
_report.timestamp_velocity = _report.timestamp_position;
_report.vel_n_m_s = msg.ned_velocity[0];
_report.vel_e_m_s = msg.ned_velocity[1];
_report.vel_d_m_s = msg.ned_velocity[2];
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
_report.vel_ned_valid = true;
report.timestamp_velocity = report.timestamp_position;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
_report.timestamp_time = _report.timestamp_position;
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
report.timestamp_time = report.timestamp_position;
report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
_report.satellites_used = msg.sats_used;
report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
}
}
src/modules/uavcan/gnss_receiver.hpp → src/modules/uavcan/sensors/gnss.hpp
+18 -17
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gnss_receiver.hpp
* @file gnss.hpp
*
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
@@ -51,12 +51,20 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
#include "sensor_bridge.hpp"
class UavcanGnssBridge : public IUavcanSensorBridge
{
public:
UavcanGnssReceiver(uavcan::INode& node);
static const char *const NAME;
int init();
UavcanGnssBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
unsigned get_num_redundant_channels() const override;
private:
/**
@@ -64,21 +72,14 @@ private:
*/
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
typedef uavcan::MethodBinder<UavcanGnssBridge*,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
int _receiver_node_id = -1;
/*
* uORB
*/
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
};
src/modules/uavcan/sensors/mag.cpp
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "mag.hpp"
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
ORB_ID(sensor_mag2)
};
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
}
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
case MAGIOCGRANGE:
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
{
auto report = ::mag_report();
report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
report.timestamp = msg.getMonotonicTimestamp().toUSec();
}
report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
publish(msg.getSrcNodeID().get(), &report);
}
src/modules/uavcan/sensors/mag.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanMagnetometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
mag_scale _scale = {};
};
src/modules/uavcan/sensors/sensor_bridge.cpp
+142
View File
@@ -0,0 +1,142 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "sensor_bridge.hpp"
#include <cassert>
#include "gnss.hpp"
#include "mag.hpp"
#include "baro.hpp"
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
list.add(new UavcanGnssBridge(node));
}
/*
* UavcanCDevSensorBridgeBase
*/
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
{
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].redunancy_channel_id >= 0) {
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
{
Channel *channel = nullptr;
// Checking if such channel already exists
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].redunancy_channel_id == redundancy_channel_id) {
channel = _channels + i;
break;
}
}
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return; // Give up immediately - saves some CPU time
}
log("adding channel %d...", redundancy_channel_id);
// Search for the first free channel
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].redunancy_channel_id < 0) {
channel = _channels + i;
break;
}
}
// No free channels left
if (channel == nullptr) {
_out_of_channels = true;
log("out of channels");
return;
}
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
log("out of class instances");
(void)unregister_class_devname(_class_devname, class_instance);
return;
}
// Publish to the appropriate topic, abort on failure
channel->orb_id = _orb_topics[class_instance];
channel->redunancy_channel_id = redundancy_channel_id;
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
*channel = Channel();
return;
}
log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(channel->orb_id, channel->orb_advert, report);
}
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].redunancy_channel_id >= 0) {
out += 1;
}
}
return out;
}
src/modules/uavcan/sensors/sensor_bridge.hpp
+124
View File
@@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <containers/List.hpp>
#include <uavcan/uavcan.hpp>
#include <drivers/device/device.h>
#include <drivers/drv_orb_dev.h>
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
virtual ~IUavcanSensorBridge() { }
/**
* Returns ASCII name of the bridge.
*/
virtual const char *get_name() const = 0;
/**
* Starts the bridge.
* @return Non-negative value on success, negative on error.
*/
virtual int init() = 0;
/**
* Returns number of active redundancy channels.
*/
virtual unsigned get_num_redundant_channels() const = 0;
/**
* Sensor bridge factory.
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
};
/**
* This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
* For example, sensor_mag0, sensor_mag1, etc.
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
int redunancy_channel_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
};
const unsigned _max_channels;
const char *const _class_devname;
orb_id_t *const _orb_topics;
Channel *const _channels;
bool _out_of_channels = false;
protected:
template <unsigned MaxChannels>
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t (&orb_topics)[MaxChannels]) :
device::CDev(name, devname),
_max_channels(MaxChannels),
_class_devname(class_devname),
_orb_topics(new orb_id_t[MaxChannels]),
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
}
/**
* Sends one measurement into appropriate ORB topic.
* New redundancy channels will be registered automatically.
* @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID)
* @param report ORB message object
*/
void publish(const int redundancy_channel_id, const void *report);
public:
virtual ~UavcanCDevSensorBridgeBase();
unsigned get_num_redundant_channels() const override;
};
src/modules/uavcan/uavcan_main.cpp
+73 -57
View File
@@ -38,6 +38,7 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <version/version.h>
@@ -65,16 +66,18 @@ UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_esc_controller(_node),
_gnss_receiver(_node)
_node_mutex(),
_esc_controller(_node)
{
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
// memset(_controls, 0, sizeof(_controls));
// memset(_poll_fds, 0, sizeof(_poll_fds));
const int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
}
UavcanNode::~UavcanNode()
@@ -99,10 +102,18 @@ UavcanNode::~UavcanNode()
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
auto next = br->getSibling();
delete br;
br = next;
}
_instance = nullptr;
}
@@ -214,11 +225,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
// Do regular cdev init
ret = CDev::init();
if (ret != OK)
if (ret != OK) {
return ret;
}
_node.setName("org.pixhawk.pixhawk");
@@ -226,14 +238,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
fill_node_info();
/* initializing the bridges UAVCAN <--> uORB */
// Actuators
ret = _esc_controller.init();
if (ret < 0)
if (ret < 0) {
return ret;
}
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
ret = br->init();
if (ret < 0) {
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
return ret;
}
warnx("sensor bridge '%s' init ok", br->get_name());
br = br->getSibling();
}
return _node.start();
}
@@ -248,6 +270,8 @@ void UavcanNode::node_spin_once()
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
@@ -291,8 +315,13 @@ int UavcanNode::run()
_groups_subscribed = _groups_required;
}
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
// this would be bad...
@@ -352,7 +381,6 @@ int UavcanNode::run()
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
@@ -376,10 +404,7 @@ int UavcanNode::run()
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@@ -520,8 +545,21 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
(void)pthread_mutex_lock(&_node_mutex);
// ESC mixer status
warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels());
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
/*
@@ -529,79 +567,57 @@ UavcanNode::print_info()
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
warnx("usage: \n"
"\tuavcan {start|status|stop}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
{
constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
if (argc < 3) {
print_usage();
::exit(1);
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Node ID
*/
const int node_id = atoi(argv[2]);
// Node ID
int32_t node_id = 0;
(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
/*
* CAN bitrate
*/
unsigned bitrate = 0;
// CAN bitrate
int32_t bitrate = 0;
(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
if (argc > 3) {
bitrate = atol(argv[3]);
}
if (bitrate <= 0) {
bitrate = DEFAULT_CAN_BITRATE;
}
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Start
*/
// Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *inst = UavcanNode::instance();
UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
return OK;
inst->print_info();
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
delete inst;
::exit(0);
}
print_usage();
src/modules/uavcan/uavcan_main.hpp
+9 -8
View File
@@ -42,8 +42,8 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
/**
* @file uavcan_main.hpp
@@ -77,12 +77,10 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
Node& get_node() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
@@ -109,8 +107,11 @@ private:
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
src/modules/uavcan/uavcan_params.c
+73
View File
@@ -0,0 +1,73 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
*
* @min 0
* @max 1
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
/**
* UAVCAN Node ID.
*
* Read the specs at http://uavcan.org to learn more about Node ID.
*
* @min 1
* @max 125
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
/**
* UAVCAN CAN bus bitrate.
*
* @min 20000
* @max 1000000
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
uavcan
+1 -1
Submodule uavcan updated: 6980ee8240...75153eb643