Anton Babushkin
11 years ago
44 changed files with 1773 additions and 44 deletions
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df |
||||
Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 |
||||
@ -0,0 +1,27 @@
@@ -0,0 +1,27 @@
|
||||
#!nsh |
||||
# |
||||
# F450-sized quadrotor with CAN |
||||
# |
||||
# Lorenz Meier <lm@inf.ethz.ch> |
||||
# |
||||
|
||||
sh /etc/init.d/4001_quad_x |
||||
|
||||
if [ $DO_AUTOCONFIG == yes ] |
||||
then |
||||
# TODO REVIEW |
||||
param set MC_ROLL_P 7.0 |
||||
param set MC_ROLLRATE_P 0.1 |
||||
param set MC_ROLLRATE_I 0.0 |
||||
param set MC_ROLLRATE_D 0.003 |
||||
param set MC_PITCH_P 7.0 |
||||
param set MC_PITCHRATE_P 0.1 |
||||
param set MC_PITCHRATE_I 0.0 |
||||
param set MC_PITCHRATE_D 0.003 |
||||
param set MC_YAW_P 2.8 |
||||
param set MC_YAWRATE_P 0.2 |
||||
param set MC_YAWRATE_I 0.0 |
||||
param set MC_YAWRATE_D 0.0 |
||||
fi |
||||
|
||||
set OUTPUT_MODE uavcan_esc |
||||
@ -0,0 +1,14 @@
@@ -0,0 +1,14 @@
|
||||
close all; |
||||
clear all; |
||||
M = importdata('px4io_v1.3.csv'); |
||||
voltage = M.data(:, 1); |
||||
counts = M.data(:, 2); |
||||
plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15); |
||||
coeffs = polyfit(counts, voltage, 1); |
||||
fittedC = linspace(min(counts), max(counts), 500); |
||||
fittedV = polyval(coeffs, fittedC); |
||||
hold on |
||||
plot(fittedC, fittedV, 'r-', 'LineWidth', 3); |
||||
|
||||
slope = coeffs(1) |
||||
y_intersection = coeffs(2) |
||||
|
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
./dsdlc_generated/ |
||||
@ -0,0 +1,138 @@
@@ -0,0 +1,138 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file esc_controller.cpp |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#include "esc_controller.hpp" |
||||
#include <systemlib/err.h> |
||||
|
||||
UavcanEscController::UavcanEscController(uavcan::INode &node) : |
||||
_node(node), |
||||
_uavcan_pub_raw_cmd(node), |
||||
_uavcan_sub_status(node), |
||||
_orb_timer(node) |
||||
{ |
||||
} |
||||
|
||||
UavcanEscController::~UavcanEscController() |
||||
{ |
||||
perf_free(_perfcnt_invalid_input); |
||||
perf_free(_perfcnt_scaling_error); |
||||
} |
||||
|
||||
int UavcanEscController::init() |
||||
{ |
||||
// ESC status subscription
|
||||
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); |
||||
if (res < 0) |
||||
{ |
||||
warnx("ESC status sub failed %i", res); |
||||
return res; |
||||
} |
||||
|
||||
// ESC status will be relayed from UAVCAN bus into ORB at this rate
|
||||
_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); |
||||
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); |
||||
|
||||
return res; |
||||
} |
||||
|
||||
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) |
||||
{ |
||||
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { |
||||
perf_count(_perfcnt_invalid_input); |
||||
return; |
||||
} |
||||
|
||||
/*
|
||||
* Rate limiting - we don't want to congest the bus |
||||
*/ |
||||
const auto timestamp = _node.getMonotonicTime(); |
||||
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { |
||||
return; |
||||
} |
||||
_prev_cmd_pub = timestamp; |
||||
|
||||
/*
|
||||
* Fill the command message |
||||
* If unarmed, we publish an empty message anyway |
||||
*/ |
||||
uavcan::equipment::esc::RawCommand msg; |
||||
|
||||
if (_armed) { |
||||
for (unsigned i = 0; i < num_outputs; i++) { |
||||
|
||||
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; |
||||
if (scaled < 1.0F) { |
||||
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
|
||||
} |
||||
|
||||
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { |
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; |
||||
perf_count(_perfcnt_scaling_error); |
||||
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { |
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; |
||||
perf_count(_perfcnt_scaling_error); |
||||
} else { |
||||
; // Correct value
|
||||
} |
||||
|
||||
msg.cmd.push_back(static_cast<unsigned>(scaled)); |
||||
} |
||||
} |
||||
|
||||
/*
|
||||
* Publish the command message to the bus |
||||
* Note that for a quadrotor it takes one CAN frame |
||||
*/ |
||||
(void)_uavcan_pub_raw_cmd.broadcast(msg); |
||||
} |
||||
|
||||
void UavcanEscController::arm_esc(bool arm) |
||||
{ |
||||
_armed = arm; |
||||
} |
||||
|
||||
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) |
||||
{ |
||||
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
|
||||
} |
||||
|
||||
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) |
||||
{ |
||||
// TODO publish to ORB
|
||||
} |
||||
@ -0,0 +1,107 @@
@@ -0,0 +1,107 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file esc_controller.hpp |
||||
* |
||||
* UAVCAN <--> ORB bridge for ESC messages: |
||||
* uavcan.equipment.esc.RawCommand |
||||
* uavcan.equipment.esc.RPMCommand |
||||
* uavcan.equipment.esc.Status |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
#include <uavcan/equipment/esc/RawCommand.hpp> |
||||
#include <uavcan/equipment/esc/Status.hpp> |
||||
#include <systemlib/perf_counter.h> |
||||
|
||||
class UavcanEscController |
||||
{ |
||||
public: |
||||
UavcanEscController(uavcan::INode& node); |
||||
~UavcanEscController(); |
||||
|
||||
int init(); |
||||
|
||||
void update_outputs(float *outputs, unsigned num_outputs); |
||||
|
||||
void arm_esc(bool arm); |
||||
|
||||
private: |
||||
/**
|
||||
* ESC status message reception will be reported via this callback. |
||||
*/ |
||||
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg); |
||||
|
||||
/**
|
||||
* ESC status will be published to ORB from this callback (fixed rate). |
||||
*/ |
||||
void orb_pub_timer_cb(const uavcan::TimerEvent &event); |
||||
|
||||
|
||||
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; |
||||
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; |
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController*, |
||||
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> |
||||
StatusCbBinder; |
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)> |
||||
TimerCbBinder; |
||||
|
||||
/*
|
||||
* libuavcan related things |
||||
*/ |
||||
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
|
||||
uavcan::INode &_node; |
||||
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd; |
||||
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status; |
||||
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; |
||||
|
||||
/*
|
||||
* ESC states |
||||
*/ |
||||
bool _armed = false; |
||||
uavcan::equipment::esc::Status _states[MAX_ESCS]; |
||||
|
||||
/*
|
||||
* Perf counters |
||||
*/ |
||||
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); |
||||
perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); |
||||
}; |
||||
@ -0,0 +1,153 @@
@@ -0,0 +1,153 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file gnss_receiver.cpp |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* @author Andrew Chambers <achamber@gmail.com> |
||||
* |
||||
*/ |
||||
|
||||
#include "gnss_receiver.hpp" |
||||
#include <systemlib/err.h> |
||||
#include <mathlib/mathlib.h> |
||||
|
||||
#define MM_PER_CM 10 // Millimeters per centimeter
|
||||
|
||||
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : |
||||
_node(node), |
||||
_uavcan_sub_status(node), |
||||
_report_pub(-1) |
||||
{ |
||||
} |
||||
|
||||
int UavcanGnssReceiver::init() |
||||
{ |
||||
int res = -1; |
||||
|
||||
// GNSS fix subscription
|
||||
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::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) |
||||
{ |
||||
_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
|
||||
const bool valid_position_covariance = !msg.position_covariance.empty(); |
||||
const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); |
||||
|
||||
if (valid_position_covariance) { |
||||
float pos_cov[9]; |
||||
msg.position_covariance.unpackSquareMatrix(pos_cov); |
||||
|
||||
// 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; |
||||
|
||||
// Vertical position uncertainty
|
||||
_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; |
||||
} else { |
||||
_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]); |
||||
|
||||
/* There is a nonlinear relationship between the velocity vector and the heading.
|
||||
* Use Jacobian to transform velocity covariance to heading covariance |
||||
* |
||||
* Nonlinear equation: |
||||
* heading = atan2(vel_e_m_s, vel_n_m_s) |
||||
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
|
||||
* |
||||
* To calculate the variance of heading from the variance of velocity, |
||||
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T |
||||
*/ |
||||
float vel_n = msg.ned_velocity[0]; |
||||
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 = |
||||
(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.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_time = _report.timestamp_position; |
||||
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
|
||||
|
||||
_report.satellites_used = msg.sats_used; |
||||
|
||||
if (_report_pub > 0) { |
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); |
||||
|
||||
} else { |
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); |
||||
} |
||||
|
||||
} |
||||
@ -0,0 +1,84 @@
@@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file gnss_receiver.hpp |
||||
* |
||||
* UAVCAN --> ORB bridge for GNSS messages: |
||||
* uavcan.equipment.gnss.Fix |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* @author Andrew Chambers <achamber@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
#include <uORB/uORB.h> |
||||
#include <uORB/topics/vehicle_gps_position.h> |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
#include <uavcan/equipment/gnss/Fix.hpp> |
||||
|
||||
class UavcanGnssReceiver |
||||
{ |
||||
public: |
||||
UavcanGnssReceiver(uavcan::INode& node); |
||||
|
||||
int init(); |
||||
|
||||
private: |
||||
/**
|
||||
* GNSS fix message will be reported via this callback. |
||||
*/ |
||||
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>&)> |
||||
FixCbBinder; |
||||
|
||||
/*
|
||||
* libuavcan related things |
||||
*/ |
||||
uavcan::INode &_node; |
||||
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status; |
||||
|
||||
/*
|
||||
* uORB |
||||
*/ |
||||
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
|
||||
orb_advert_t _report_pub; ///< uORB pub for gnss position
|
||||
|
||||
}; |
||||
@ -0,0 +1,74 @@
@@ -0,0 +1,74 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# UAVCAN <--> uORB bridge
|
||||
#
|
||||
|
||||
MODULE_COMMAND = uavcan
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
SRCS += uavcan_main.cpp \
|
||||
uavcan_clock.cpp \
|
||||
esc_controller.cpp \
|
||||
gnss_receiver.cpp
|
||||
|
||||
#
|
||||
# libuavcan
|
||||
#
|
||||
include $(UAVCAN_DIR)/libuavcan/include.mk |
||||
SRCS += $(LIBUAVCAN_SRC)
|
||||
INCLUDE_DIRS += $(LIBUAVCAN_INC)
|
||||
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
|
||||
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
|
||||
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS |
||||
|
||||
#
|
||||
# libuavcan drivers for STM32
|
||||
#
|
||||
include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk |
||||
SRCS += $(LIBUAVCAN_STM32_SRC)
|
||||
INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
|
||||
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 |
||||
|
||||
#
|
||||
# Invoke DSDL compiler
|
||||
# TODO: Add make target for this, or invoke dsdlc manually.
|
||||
# The second option assumes that the generated headers shall be saved
|
||||
# under the version control, which may be undesirable.
|
||||
# The first option requires any Python and the Python Mako library for the sources to be built.
|
||||
#
|
||||
$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) |
||||
INCLUDE_DIRS += dsdlc_generated
|
||||
@ -0,0 +1,79 @@
@@ -0,0 +1,79 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
/**
|
||||
* @file uavcan_clock.cpp |
||||
* |
||||
* Implements a clock for the CAN node. |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
namespace uavcan_stm32 |
||||
{ |
||||
namespace clock |
||||
{ |
||||
|
||||
uavcan::MonotonicTime getMonotonic() |
||||
{ |
||||
return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); |
||||
} |
||||
|
||||
uavcan::UtcTime getUtc() |
||||
{ |
||||
return uavcan::UtcTime(); |
||||
} |
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment) |
||||
{ |
||||
(void)adjustment; |
||||
} |
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt() |
||||
{ |
||||
return 0; |
||||
} |
||||
|
||||
} // namespace clock
|
||||
|
||||
SystemClock &SystemClock::instance() |
||||
{ |
||||
static SystemClock inst; |
||||
return inst; |
||||
} |
||||
|
||||
} |
||||
|
||||
@ -0,0 +1,582 @@
@@ -0,0 +1,582 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <cstdlib> |
||||
#include <cstring> |
||||
#include <fcntl.h> |
||||
#include <systemlib/err.h> |
||||
#include <systemlib/systemlib.h> |
||||
#include <systemlib/mixer/mixer.h> |
||||
#include <arch/board/board.h> |
||||
#include <arch/chip/chip.h> |
||||
|
||||
#include <drivers/drv_hrt.h> |
||||
#include <drivers/drv_pwm_output.h> |
||||
|
||||
#include "uavcan_main.hpp" |
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp |
||||
* |
||||
* Implements basic functinality of UAVCAN node. |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
/*
|
||||
* UavcanNode |
||||
*/ |
||||
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) |
||||
{ |
||||
_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));
|
||||
} |
||||
|
||||
UavcanNode::~UavcanNode() |
||||
{ |
||||
if (_task != -1) { |
||||
/* tell the task we want it to go away */ |
||||
_task_should_exit = true; |
||||
|
||||
unsigned i = 10; |
||||
|
||||
do { |
||||
/* wait 5ms - it should wake every 10ms or so worst-case */ |
||||
usleep(5000); |
||||
|
||||
/* if we have given up, kill it */ |
||||
if (--i == 0) { |
||||
task_delete(_task); |
||||
break; |
||||
} |
||||
|
||||
} while (_task != -1); |
||||
} |
||||
|
||||
/* clean up the alternate device node */ |
||||
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||
|
||||
::close(_armed_sub); |
||||
|
||||
_instance = nullptr; |
||||
} |
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) |
||||
{ |
||||
if (_instance != nullptr) { |
||||
warnx("Already started"); |
||||
return -1; |
||||
} |
||||
|
||||
/*
|
||||
* GPIO config. |
||||
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. |
||||
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to |
||||
* fail during initialization. |
||||
*/ |
||||
stm32_configgpio(GPIO_CAN1_RX); |
||||
stm32_configgpio(GPIO_CAN1_TX); |
||||
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); |
||||
stm32_configgpio(GPIO_CAN2_TX); |
||||
|
||||
/*
|
||||
* CAN driver init |
||||
*/ |
||||
static CanInitHelper can; |
||||
static bool can_initialized = false; |
||||
|
||||
if (!can_initialized) { |
||||
const int can_init_res = can.init(bitrate); |
||||
|
||||
if (can_init_res < 0) { |
||||
warnx("CAN driver init failed %i", can_init_res); |
||||
return can_init_res; |
||||
} |
||||
|
||||
can_initialized = true; |
||||
} |
||||
|
||||
/*
|
||||
* Node init |
||||
*/ |
||||
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); |
||||
|
||||
if (_instance == nullptr) { |
||||
warnx("Out of memory"); |
||||
return -1; |
||||
} |
||||
|
||||
const int node_init_res = _instance->init(node_id); |
||||
|
||||
if (node_init_res < 0) { |
||||
delete _instance; |
||||
_instance = nullptr; |
||||
warnx("Node init failed %i", node_init_res); |
||||
return node_init_res; |
||||
} |
||||
|
||||
/*
|
||||
* Start the task. Normally it should never exit. |
||||
*/ |
||||
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; |
||||
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, |
||||
static_cast<main_t>(run_trampoline), nullptr); |
||||
|
||||
if (_instance->_task < 0) { |
||||
warnx("start failed: %d", errno); |
||||
return -errno; |
||||
} |
||||
|
||||
return OK; |
||||
} |
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id) |
||||
{ |
||||
int ret = -1; |
||||
|
||||
/* do regular cdev init */ |
||||
ret = CDev::init(); |
||||
|
||||
if (ret != OK) |
||||
return ret; |
||||
|
||||
ret = _esc_controller.init(); |
||||
if (ret < 0) |
||||
return ret; |
||||
|
||||
ret = _gnss_receiver.init(); |
||||
if (ret < 0) |
||||
return ret; |
||||
|
||||
uavcan::protocol::SoftwareVersion swver; |
||||
swver.major = 12; // TODO fill version info
|
||||
swver.minor = 34; |
||||
_node.setSoftwareVersion(swver); |
||||
|
||||
uavcan::protocol::HardwareVersion hwver; |
||||
hwver.major = 42; // TODO fill version info
|
||||
hwver.minor = 42; |
||||
_node.setHardwareVersion(hwver); |
||||
|
||||
_node.setName("org.pixhawk"); // Huh?
|
||||
|
||||
_node.setNodeID(node_id); |
||||
|
||||
return _node.start(); |
||||
} |
||||
|
||||
void UavcanNode::node_spin_once() |
||||
{ |
||||
const int spin_res = _node.spin(uavcan::MonotonicTime()); |
||||
if (spin_res < 0) { |
||||
warnx("node spin error %i", spin_res); |
||||
} |
||||
} |
||||
|
||||
int UavcanNode::run() |
||||
{ |
||||
const unsigned PollTimeoutMs = 50; |
||||
|
||||
// XXX figure out the output count
|
||||
_output_count = 2; |
||||
|
||||
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed)); |
||||
|
||||
actuator_outputs_s outputs; |
||||
memset(&outputs, 0, sizeof(outputs)); |
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); |
||||
if (busevent_fd < 0) |
||||
{ |
||||
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); |
||||
_task_should_exit = true; |
||||
} |
||||
|
||||
/*
|
||||
* XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); |
||||
* IO multiplexing shall be done here. |
||||
*/ |
||||
|
||||
_node.setStatusOk(); |
||||
|
||||
while (!_task_should_exit) { |
||||
|
||||
if (_groups_subscribed != _groups_required) { |
||||
subscribe(); |
||||
_groups_subscribed = _groups_required; |
||||
/*
|
||||
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). |
||||
* Please note that with such multiplexing it is no longer possible to rely only on |
||||
* the value returned from poll() to detect whether actuator control has timed out or not. |
||||
* Instead, all ORB events need to be checked individually (see below). |
||||
*/ |
||||
_poll_fds[_poll_fds_num] = ::pollfd(); |
||||
_poll_fds[_poll_fds_num].fd = busevent_fd; |
||||
_poll_fds[_poll_fds_num].events = POLLIN; |
||||
_poll_fds_num += 1; |
||||
} |
||||
|
||||
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); |
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
// this would be bad...
|
||||
if (poll_ret < 0) { |
||||
log("poll error %d", errno); |
||||
continue; |
||||
} else { |
||||
// get controls for required topics
|
||||
bool controls_updated = false; |
||||
unsigned poll_id = 0; |
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { |
||||
if (_control_subs[i] > 0) { |
||||
if (_poll_fds[poll_id].revents & POLLIN) { |
||||
controls_updated = true; |
||||
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); |
||||
} |
||||
poll_id++; |
||||
} |
||||
} |
||||
|
||||
if (!controls_updated) { |
||||
// timeout: no control data, switch to failsafe values
|
||||
// XXX trigger failsafe
|
||||
} |
||||
|
||||
//can we mix?
|
||||
if (controls_updated && (_mixers != nullptr)) { |
||||
|
||||
// XXX one output group has 8 outputs max,
|
||||
// but this driver could well serve multiple groups.
|
||||
unsigned num_outputs_max = 8; |
||||
|
||||
// Do mixing
|
||||
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); |
||||
outputs.timestamp = hrt_absolute_time(); |
||||
|
||||
// iterate actuators
|
||||
for (unsigned i = 0; i < outputs.noutputs; i++) { |
||||
// last resort: catch NaN, INF and out-of-band errors
|
||||
if (!isfinite(outputs.output[i])) { |
||||
/*
|
||||
* Value is NaN, INF or out of band - set to the minimum value. |
||||
* This will be clearly visible on the servo status and will limit the risk of accidentally |
||||
* spinning motors. It would be deadly in flight. |
||||
*/ |
||||
outputs.output[i] = -1.0f; |
||||
} |
||||
|
||||
// limit outputs to valid range
|
||||
|
||||
// never go below min
|
||||
if (outputs.output[i] < -1.0f) { |
||||
outputs.output[i] = -1.0f; |
||||
} |
||||
|
||||
// never go below max
|
||||
if (outputs.output[i] > 1.0f) { |
||||
outputs.output[i] = 1.0f; |
||||
} |
||||
} |
||||
|
||||
// Output to the bus
|
||||
_esc_controller.update_outputs(outputs.output, outputs.noutputs); |
||||
} |
||||
|
||||
} |
||||
|
||||
// Check arming state
|
||||
bool updated = false; |
||||
orb_check(_armed_sub, &updated); |
||||
|
||||
if (updated) { |
||||
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); |
||||
|
||||
// Update the armed status and check that we're not locked down
|
||||
bool set_armed = _armed.armed && !_armed.lockdown; |
||||
|
||||
arm_actuators(set_armed); |
||||
} |
||||
} |
||||
|
||||
teardown(); |
||||
warnx("exiting."); |
||||
|
||||
exit(0); |
||||
} |
||||
|
||||
int |
||||
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; |
||||
|
||||
input = controls[control_group].control[control_index]; |
||||
return 0; |
||||
} |
||||
|
||||
int |
||||
UavcanNode::teardown() |
||||
{ |
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { |
||||
if (_control_subs[i] > 0) { |
||||
::close(_control_subs[i]); |
||||
_control_subs[i] = -1; |
||||
} |
||||
} |
||||
return ::close(_armed_sub); |
||||
} |
||||
|
||||
int |
||||
UavcanNode::arm_actuators(bool arm) |
||||
{ |
||||
_is_armed = arm; |
||||
_esc_controller.arm_esc(arm); |
||||
return OK; |
||||
} |
||||
|
||||
void |
||||
UavcanNode::subscribe() |
||||
{ |
||||
// Subscribe/unsubscribe to required actuator control groups
|
||||
uint32_t sub_groups = _groups_required & ~_groups_subscribed; |
||||
uint32_t unsub_groups = _groups_subscribed & ~_groups_required; |
||||
_poll_fds_num = 0; |
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { |
||||
if (sub_groups & (1 << i)) { |
||||
warnx("subscribe to actuator_controls_%d", i); |
||||
_control_subs[i] = orb_subscribe(_control_topics[i]); |
||||
} |
||||
if (unsub_groups & (1 << i)) { |
||||
warnx("unsubscribe from actuator_controls_%d", i); |
||||
::close(_control_subs[i]); |
||||
_control_subs[i] = -1; |
||||
} |
||||
|
||||
if (_control_subs[i] > 0) { |
||||
_poll_fds[_poll_fds_num].fd = _control_subs[i]; |
||||
_poll_fds[_poll_fds_num].events = POLLIN; |
||||
_poll_fds_num++; |
||||
} |
||||
} |
||||
} |
||||
|
||||
int |
||||
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) |
||||
{ |
||||
int ret = OK; |
||||
|
||||
lock(); |
||||
|
||||
switch (cmd) { |
||||
case PWM_SERVO_ARM: |
||||
arm_actuators(true); |
||||
break; |
||||
|
||||
case PWM_SERVO_SET_ARM_OK: |
||||
case PWM_SERVO_CLEAR_ARM_OK: |
||||
case PWM_SERVO_SET_FORCE_SAFETY_OFF: |
||||
// these are no-ops, as no safety switch
|
||||
break; |
||||
|
||||
case PWM_SERVO_DISARM: |
||||
arm_actuators(false); |
||||
break; |
||||
|
||||
case MIXERIOCGETOUTPUTCOUNT: |
||||
*(unsigned *)arg = _output_count; |
||||
break; |
||||
|
||||
case MIXERIOCRESET: |
||||
if (_mixers != nullptr) { |
||||
delete _mixers; |
||||
_mixers = nullptr; |
||||
_groups_required = 0; |
||||
} |
||||
|
||||
break; |
||||
|
||||
case MIXERIOCLOADBUF: { |
||||
const char *buf = (const char *)arg; |
||||
unsigned buflen = strnlen(buf, 1024); |
||||
|
||||
if (_mixers == nullptr) |
||||
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls); |
||||
|
||||
if (_mixers == nullptr) { |
||||
_groups_required = 0; |
||||
ret = -ENOMEM; |
||||
|
||||
} else { |
||||
|
||||
ret = _mixers->load_from_buf(buf, buflen); |
||||
|
||||
if (ret != 0) { |
||||
warnx("mixer load failed with %d", ret); |
||||
delete _mixers; |
||||
_mixers = nullptr; |
||||
_groups_required = 0; |
||||
ret = -EINVAL; |
||||
} else { |
||||
|
||||
_mixers->groups_required(_groups_required); |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
|
||||
default: |
||||
ret = -ENOTTY; |
||||
break; |
||||
} |
||||
|
||||
unlock(); |
||||
|
||||
if (ret == -ENOTTY) { |
||||
ret = CDev::ioctl(filp, cmd, arg); |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
void |
||||
UavcanNode::print_info() |
||||
{ |
||||
if (!_instance) { |
||||
warnx("not running, start first"); |
||||
} |
||||
|
||||
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); |
||||
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); |
||||
} |
||||
|
||||
/*
|
||||
* App entry point |
||||
*/ |
||||
static void print_usage() |
||||
{ |
||||
warnx("usage: uavcan start <node_id> [can_bitrate]"); |
||||
} |
||||
|
||||
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); |
||||
} |
||||
|
||||
/*
|
||||
* Node ID |
||||
*/ |
||||
const int node_id = atoi(argv[2]); |
||||
|
||||
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; |
||||
} |
||||
|
||||
if (UavcanNode::instance()) { |
||||
errx(1, "already started"); |
||||
} |
||||
|
||||
/*
|
||||
* 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(); |
||||
|
||||
if (!inst) { |
||||
errx(1, "application not running"); |
||||
} |
||||
|
||||
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { |
||||
|
||||
inst->print_info(); |
||||
return OK; |
||||
} |
||||
|
||||
if (!std::strcmp(argv[1], "stop")) { |
||||
|
||||
delete inst; |
||||
inst = nullptr; |
||||
return OK; |
||||
} |
||||
|
||||
print_usage(); |
||||
::exit(1); |
||||
} |
||||
@ -0,0 +1,123 @@
@@ -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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp> |
||||
#include <drivers/device/device.h> |
||||
|
||||
#include <uORB/topics/actuator_controls.h> |
||||
#include <uORB/topics/actuator_outputs.h> |
||||
#include <uORB/topics/actuator_armed.h> |
||||
|
||||
#include "esc_controller.hpp" |
||||
#include "gnss_receiver.hpp" |
||||
|
||||
/**
|
||||
* @file uavcan_main.hpp |
||||
* |
||||
* Defines basic functinality of UAVCAN node. |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 |
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" |
||||
|
||||
/**
|
||||
* A UAVCAN node. |
||||
*/ |
||||
class UavcanNode : public device::CDev |
||||
{ |
||||
static constexpr unsigned MemPoolSize = 10752; |
||||
static constexpr unsigned RxQueueLenPerIface = 64; |
||||
static constexpr unsigned StackSize = 3000; |
||||
|
||||
public: |
||||
typedef uavcan::Node<MemPoolSize> Node; |
||||
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper; |
||||
|
||||
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); |
||||
|
||||
virtual ~UavcanNode(); |
||||
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg); |
||||
|
||||
static int start(uavcan::NodeID node_id, uint32_t bitrate); |
||||
|
||||
Node& getNode() { return _node; } |
||||
|
||||
static int control_callback(uintptr_t handle, |
||||
uint8_t control_group, |
||||
uint8_t control_index, |
||||
float &input); |
||||
|
||||
void subscribe(); |
||||
|
||||
int teardown(); |
||||
int arm_actuators(bool arm); |
||||
|
||||
void print_info(); |
||||
|
||||
static UavcanNode* instance() { return _instance; } |
||||
|
||||
private: |
||||
int init(uavcan::NodeID node_id); |
||||
void node_spin_once(); |
||||
int run(); |
||||
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
int _armed_sub = -1; ///< uORB subscription of the arming status
|
||||
actuator_armed_s _armed; ///< the arming request of the system
|
||||
bool _is_armed = false; ///< the arming status of the actuators on the bus
|
||||
|
||||
unsigned _output_count = 0; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
Node _node; ///< library instance
|
||||
UavcanEscController _esc_controller; |
||||
UavcanGnssReceiver _gnss_receiver; |
||||
|
||||
MixerGroup *_mixers = nullptr; |
||||
|
||||
uint32_t _groups_required = 0; |
||||
uint32_t _groups_subscribed = 0; |
||||
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; |
||||
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; |
||||
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; |
||||
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
|
||||
unsigned _poll_fds_num = 0; |
||||
}; |
||||
Loading…
Reference in new issue