diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 16:26:42 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 16:26:42 +0200 |
commit | e52f7770be443301a40ee6df40498c64083b4884 (patch) | |
tree | 4ce8013d008e2e78cbca76fba72813bc4f918c02 /src/modules | |
parent | bdccd69030e56381d906afeabc8305dbe18e2de6 (diff) | |
parent | 3454c42596d8c0c1bec59dec87f4a53910f83731 (diff) | |
download | px4-firmware-e52f7770be443301a40ee6df40498c64083b4884.tar.gz px4-firmware-e52f7770be443301a40ee6df40498c64083b4884.tar.bz2 px4-firmware-e52f7770be443301a40ee6df40498c64083b4884.zip |
Merge remote-tracking branch 'upstream/master' into obcfailsafe
Diffstat (limited to 'src/modules')
19 files changed, 922 insertions, 153 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..339b11bbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -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; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4506942ab..098ff1a3d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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; + } } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index af4d46a36..5a92004a6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f40034d79..cdcb428dd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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 */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index cd48d3cb2..7342fcf04 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -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 */ diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp index 406eba88c..223d94731 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -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) : diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp index 559ede561..cf0988210 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file esc_controller.hpp + * @file esc.hpp * * UAVCAN <--> ORB bridge for ESC messages: * uavcan.equipment.esc.RawCommand diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f2468..f92bc754f 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -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 diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 000000000..80c5e3828 --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -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); +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 000000000..9d470219e --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -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; +}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp index ba1fe5e49..8548660fe 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -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; +} + +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; + 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); } } diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp index 18df8da2f..c2b6e4195 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -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; + + UavcanGnssBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; - int init(); + 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 }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 000000000..8e6a9a22f --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -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); +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 000000000..6d413a8f7 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -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 = {}; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 000000000..a98596f9c --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -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; +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 000000000..a13d0ab35 --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -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; +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6a..482fec1e0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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; - - if (argc > 3) { - bitrate = atol(argv[3]); - } - - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &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(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b..be7db9741 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -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; diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 000000000..e6ea8a8fb --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -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); + + + |