fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 09:34:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged upstream/master

Browse Source
This commit is contained in:
Lorenz Meier 2014-08-26 09:29:55 +02:00
commit 9434633912
25 changed files with 1036 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ROMFS/px4fmu_common/init.d/rc.uavcan Normal file
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

12
ROMFS/px4fmu_common/init.d/rcS
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
#

6
Tools/px_uploader.py
View File

@ -178,9 +178,9 @@ class uploader(object):
MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
def __init__(self, portname, baudrate):
def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
# open the port, keep the default timeout short so we can poll quickly
self.port = serial.Serial(portname, baudrate, timeout=2.0)
self.port = serial.Serial(portname, baudrate)
self.otp = b''
self.sn = b''
@ -195,7 +195,7 @@ class uploader(object):
def __recv(self, count=1):
c = self.port.read(count)
if len(c) < 1:
raise RuntimeError("timeout waiting for data")
raise RuntimeError("timeout waiting for data (%u bytes)", count)
# print("recv " + binascii.hexlify(c))
return c

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
MODULES += drivers/pca8574
MODULES += drivers/px4flow
# Needs to be burned to the ground and re-written; for now,

31
src/drivers/drv_px4flow.h
View File

@ -46,37 +46,6 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* @addtogroup topics
* @{
*/
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*
* @warning If possible the usage of the raw flow and performing rotation-compensation
* using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
uint64_t timestamp; /**< in microseconds since system start */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/**
* @}
*/
/*
* ObjDev tag for px4flow data.
*/

68
src/drivers/px4flow/px4flow.cpp
View File

@ -37,7 +37,7 @@
*
* Driver for the PX4FLOW module connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
//#include <uORB/topics/optical_flow.h>
#include <uORB/topics/optical_flow.h>
#include <board_config.h>
@ -80,7 +80,7 @@
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
@ -136,13 +136,13 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
@ -151,7 +151,7 @@ private:
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
@ -159,12 +159,12 @@ private:
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@ -179,8 +179,8 @@ private:
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
{
// enable debug() calls
_debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@ -226,13 +226,13 @@ PX4FLOW::init()
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(px4flow_report));
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
if (_reports == nullptr)
goto out;
/* get a publish handle on the px4flow topic */
struct px4flow_report zero_report;
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
/* 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;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct px4flow_report);
struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
unsigned count = buflen / sizeof(struct optical_flow_s);
struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -425,7 +425,7 @@ PX4FLOW::measure()
return ret;
}
ret = OK;
return ret;
}
@ -433,14 +433,14 @@ int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0)
{
log("error reading from sensor: %d", ret);
@ -448,7 +448,7 @@ PX4FLOW::collect()
perf_end(_sample_perf);
return ret;
}
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
@ -466,7 +466,7 @@ PX4FLOW::collect()
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct px4flow_report report;
struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
@ -503,7 +503,7 @@ PX4FLOW::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
@ -644,7 +644,7 @@ start()
fail:
if (g_dev != nullptr)
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
@ -678,7 +678,7 @@ void stop()
void
test()
{
struct px4flow_report report;
struct optical_flow_s report;
ssize_t sz;
int ret;
@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "start"))
px4flow::start();
/*
* Stop the driver
*/

19
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -145,6 +145,7 @@ private:
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
_setpoint_valid(false)
_setpoint_valid(false),
_debug(false)
{
/* safely initialize structs */
_att = {};
@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed.true_airspeed_m_s;
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}

8
src/modules/mavlink/mavlink_main.cpp
View File

@ -1396,15 +1396,15 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 15.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 0.5f);
configure_stream("OPTICAL_FLOW", 20.0f);
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
// XXX OBC change back
// XXX OBC change back: We need to be bandwidth-efficient here too
configure_stream("ATTITUDE", 50.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
configure_stream("ATTITUDE_TARGET", 50.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f);
break;

21
src/modules/sdlog2/sdlog2.c
View File

@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct vision_position_estimate vision_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_VISN_s log_VISN;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int vision_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- VISION POSITION --- */
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
log_msg.msg_type = LOG_VISN_MSG;
log_msg.body.log_VISN.x = buf.vision_pos.x;
log_msg.body.log_VISN.y = buf.vision_pos.y;
log_msg.body.log_VISN.z = buf.vision_pos.z;
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
LOGBUFFER_WRITE_AND_COUNT(VISN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {

15
src/modules/sdlog2/sdlog2_messages.h
View File

@ -391,6 +391,20 @@ struct log_TEL_s {
uint64_t heartbeat_time;
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 38
struct log_VISN_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float qx;
float qy;
float qz;
float qw;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),

4
src/modules/uavcan/esc_controller.cpp → src/modules/uavcan/actuators/esc.cpp
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) :

2
src/modules/uavcan/esc_controller.hpp → src/modules/uavcan/actuators/esc.hpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file esc_controller.hpp
* @file esc.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand

17
src/modules/uavcan/module.mk
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

117
src/modules/uavcan/sensors/baro.cpp Normal file
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);
}

68
src/modules/uavcan/sensors/baro.hpp Normal file
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;
};

102
src/modules/uavcan/gnss_receiver.cpp → src/modules/uavcan/sensors/gnss.cpp
View File

@ -32,52 +32,74 @@
****************************************************************************/
/**
* @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::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
}
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 +112,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 +140,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);
}
}

37
src/modules/uavcan/gnss_receiver.hpp → src/modules/uavcan/sensors/gnss.hpp
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,22 @@
#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;
void print_status() const override;
private:
/**
@ -64,21 +74,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
};

123
src/modules/uavcan/sensors/mag.cpp Normal file
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);
}

68
src/modules/uavcan/sensors/mag.hpp Normal file
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 = {};
};

158
src/modules/uavcan/sensors/sensor_bridge.cpp Normal file
View File

@ -0,0 +1,158 @@
/****************************************************************************
*
* 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].node_id >= 0) {
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
{
assert(report != nullptr);
Channel *channel = nullptr;
// Checking if such channel already exists
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id == node_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...", node_id);
// Search for the first free channel
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_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->node_id = node_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->node_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].node_id >= 0) {
out += 1;
}
}
return out;
}
void UavcanCDevSensorBridgeBase::print_status() const
{
printf("devname: %s\n", _class_devname);
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}
}
}

131
src/modules/uavcan/sensors/sensor_bridge.hpp Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
*
* 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;
/**
* Prints current status in a human readable format to stdout.
*/
virtual void print_status() 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 node_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 node_id Sensor's Node ID
* @param report Pointer to ORB message object
*/
void publish(const int node_id, const void *report);
public:
virtual ~UavcanCDevSensorBridgeBase();
unsigned get_num_redundant_channels() const override;
void print_status() const override;
};

132
src/modules/uavcan/uavcan_main.cpp
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,23 @@ 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
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
printf("Sensor '%s':\n", br->get_name());
br->print_status();
printf("\n");
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
/*
@ -529,79 +569,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();

17
src/modules/uavcan/uavcan_main.hpp
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;

73
src/modules/uavcan/uavcan_params.c Normal file
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);

2
uavcan

@ -1 +1 @@
Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520
Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0