2017-04-02 11:55:40 -03:00
|
|
|
/*
|
2018-07-18 06:26:48 -03:00
|
|
|
* This file is free software: you can redistribute it and/or modify it
|
|
|
|
* under the terms of the GNU General Public License as published by the
|
|
|
|
* Free Software Foundation, either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
2017-04-02 11:55:40 -03:00
|
|
|
*
|
2018-07-18 06:26:48 -03:00
|
|
|
* This file is distributed in the hope that it will be useful, but
|
|
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
|
|
* See the GNU General Public License for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License along
|
|
|
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*
|
|
|
|
* Author: Eugene Shamaev, Siddharth Bharat Purohit
|
2017-04-02 11:55:40 -03:00
|
|
|
*/
|
|
|
|
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#if HAL_WITH_UAVCAN
|
|
|
|
#include "AP_UAVCAN.h"
|
|
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
|
2017-05-06 06:13:44 -03:00
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
|
|
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
|
|
|
|
|
2018-07-18 06:26:48 -03:00
|
|
|
#include <uavcan/transport/can_acceptance_filter_configurator.hpp>
|
2018-02-16 18:32:50 -04:00
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
#include <uavcan/equipment/actuator/ArrayCommand.hpp>
|
|
|
|
#include <uavcan/equipment/actuator/Command.hpp>
|
|
|
|
#include <uavcan/equipment/actuator/Status.hpp>
|
2018-02-16 18:32:50 -04:00
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
#include <uavcan/equipment/esc/RawCommand.hpp>
|
2018-01-17 03:18:00 -04:00
|
|
|
#include <uavcan/equipment/indication/LightsCommand.hpp>
|
|
|
|
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
|
|
|
|
#include <uavcan/equipment/indication/RGB565.hpp>
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-18 06:26:48 -03:00
|
|
|
#include <AP_Baro/AP_Baro_UAVCAN.h>
|
|
|
|
#include <AP_GPS/AP_GPS_UAVCAN.h>
|
|
|
|
#include <AP_BattMonitor/AP_BattMonitor_UAVCAN.h>
|
|
|
|
#include <AP_Compass/AP_Compass_UAVCAN.h>
|
2018-09-05 03:46:12 -03:00
|
|
|
#include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
|
2018-02-16 18:32:50 -04:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
#define LED_DELAY_US 50000
|
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)
|
2017-04-02 11:55:40 -03:00
|
|
|
|
|
|
|
// Translation of all messages from UAVCAN structures into AP structures is done
|
|
|
|
// in AP_UAVCAN and not in corresponding drivers.
|
|
|
|
// The overhead of including definitions of DSDL is very high and it is best to
|
|
|
|
// concentrate in one place.
|
|
|
|
|
|
|
|
// table of user settable CAN bus parameters
|
|
|
|
const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
|
|
|
|
// @Param: NODE
|
2017-05-06 06:13:44 -03:00
|
|
|
// @DisplayName: UAVCAN node that is used for this network
|
2017-04-02 11:55:40 -03:00
|
|
|
// @Description: UAVCAN node should be set implicitly
|
|
|
|
// @Range: 1 250
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10),
|
|
|
|
|
2017-05-06 06:13:44 -03:00
|
|
|
// @Param: SRV_BM
|
|
|
|
// @DisplayName: RC Out channels to be transmitted as servo over UAVCAN
|
|
|
|
// @Description: Bitmask with one set for channel to be transmitted as a servo command over UAVCAN
|
|
|
|
// @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15
|
|
|
|
// @User: Advanced
|
2018-05-14 20:41:17 -03:00
|
|
|
AP_GROUPINFO("SRV_BM", 2, AP_UAVCAN, _servo_bm, 0),
|
2017-05-06 06:13:44 -03:00
|
|
|
|
|
|
|
// @Param: ESC_BM
|
|
|
|
// @DisplayName: RC Out channels to be transmitted as ESC over UAVCAN
|
|
|
|
// @Description: Bitmask with one set for channel to be transmitted as a ESC command over UAVCAN
|
|
|
|
// @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16
|
|
|
|
// @User: Advanced
|
2018-05-14 20:41:17 -03:00
|
|
|
AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 0),
|
2017-05-06 06:13:44 -03:00
|
|
|
|
2018-05-24 07:23:00 -03:00
|
|
|
// @Param: SRV_RT
|
|
|
|
// @DisplayName: Servo output rate
|
|
|
|
// @Description: Maximum transmit rate for servo outputs
|
|
|
|
// @Range: 1 200
|
|
|
|
// @Units: Hz
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50),
|
2018-07-18 00:08:08 -03:00
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
2018-05-22 00:37:46 -03:00
|
|
|
// this is the timeout in milliseconds for periodic message types. We
|
|
|
|
// set this to 1 to minimise resend of stale msgs
|
2018-05-24 07:23:37 -03:00
|
|
|
#define CAN_PERIODIC_TX_TIMEOUT_MS 2
|
2018-05-22 00:37:46 -03:00
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
// publisher interfaces
|
2017-05-06 06:13:44 -03:00
|
|
|
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
|
|
|
|
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
|
2018-01-17 03:18:00 -04:00
|
|
|
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
|
2017-04-02 11:55:40 -03:00
|
|
|
|
|
|
|
AP_UAVCAN::AP_UAVCAN() :
|
2017-05-06 06:13:44 -03:00
|
|
|
_node_allocator(
|
2017-04-02 11:55:40 -03:00
|
|
|
UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
|
|
|
|
{
|
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
|
2018-03-09 11:36:21 -04:00
|
|
|
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
2018-05-24 07:23:00 -03:00
|
|
|
_SRV_conf[i].esc_pending = false;
|
|
|
|
_SRV_conf[i].servo_pending = false;
|
2017-04-02 11:55:40 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
debug_uavcan(2, "AP_UAVCAN constructed\n\r");
|
|
|
|
}
|
|
|
|
|
|
|
|
AP_UAVCAN::~AP_UAVCAN()
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index)
|
|
|
|
{
|
|
|
|
if (driver_index >= AP::can().get_num_drivers() ||
|
|
|
|
AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_UAVCAN) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
return static_cast<AP_UAVCAN*>(AP::can().get_driver(driver_index));
|
|
|
|
}
|
|
|
|
|
2018-11-16 05:12:07 -04:00
|
|
|
void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
|
2017-04-02 11:55:40 -03:00
|
|
|
{
|
2018-03-09 04:31:17 -04:00
|
|
|
if (_initialized) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(2, "UAVCAN: init called more than once\n\r");
|
2018-02-27 20:02:09 -04:00
|
|
|
return;
|
2018-03-09 04:31:17 -04:00
|
|
|
}
|
2018-02-27 20:02:09 -04:00
|
|
|
|
|
|
|
_driver_index = driver_index;
|
|
|
|
|
|
|
|
AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
|
|
|
|
if (can_mgr == nullptr) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(2, "UAVCAN: init called for inexisting CAN driver\n\r");
|
2018-02-27 20:02:09 -04:00
|
|
|
return;
|
2018-03-09 04:31:17 -04:00
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
if (!can_mgr->is_initialized()) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(1, "UAVCAN: CAN driver not initialized\n\r");
|
2018-02-27 20:02:09 -04:00
|
|
|
return;
|
2018-03-09 04:31:17 -04:00
|
|
|
}
|
2017-05-06 06:13:44 -03:00
|
|
|
|
2018-07-18 00:08:08 -03:00
|
|
|
uavcan::ICanDriver* driver = can_mgr->get_driver();
|
|
|
|
if (driver == nullptr) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(2, "UAVCAN: can't get UAVCAN interface driver\n\r");
|
2018-07-18 00:08:08 -03:00
|
|
|
return;
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
_node = new uavcan::Node<0>(*driver, SystemClock::instance(), _node_allocator);
|
2018-07-18 00:08:08 -03:00
|
|
|
|
|
|
|
if (_node == nullptr) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(1, "UAVCAN: couldn't allocate node\n\r");
|
2018-02-27 20:02:09 -04:00
|
|
|
return;
|
2018-03-09 04:31:17 -04:00
|
|
|
}
|
2018-02-27 20:02:09 -04:00
|
|
|
|
2018-07-18 00:08:08 -03:00
|
|
|
if (_node->isStarted()) {
|
2018-07-20 10:46:29 -03:00
|
|
|
debug_uavcan(2, "UAVCAN: node was already started?\n\r");
|
2018-02-27 20:02:09 -04:00
|
|
|
return;
|
2018-03-09 04:31:17 -04:00
|
|
|
}
|
2018-02-27 20:02:09 -04:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
uavcan::NodeID self_node_id(_uavcan_node);
|
2018-07-18 00:08:08 -03:00
|
|
|
_node->setNodeID(self_node_id);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
char ndname[20];
|
2018-02-27 20:02:09 -04:00
|
|
|
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
uavcan::NodeStatusProvider::NodeName name(ndname);
|
2018-07-18 00:08:08 -03:00
|
|
|
_node->setName(name);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
|
|
|
|
sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
|
|
|
|
sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
|
2018-07-18 00:08:08 -03:00
|
|
|
_node->setSoftwareVersion(sw_version);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
|
2017-05-06 06:13:44 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
|
|
|
|
hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
|
2018-09-28 12:32:02 -03:00
|
|
|
|
2018-10-12 05:15:19 -03:00
|
|
|
const uint8_t uid_buf_len = hw_version.unique_id.capacity();
|
|
|
|
uint8_t uid_len = uid_buf_len;
|
|
|
|
uint8_t unique_id[uid_buf_len];
|
2018-09-28 12:32:02 -03:00
|
|
|
|
2018-10-12 05:15:19 -03:00
|
|
|
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
|
|
|
|
uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
|
2018-11-26 23:27:20 -04:00
|
|
|
}
|
|
|
|
_node->setHardwareVersion(hw_version);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-11-26 23:27:20 -04:00
|
|
|
int start_res = _node->start();
|
|
|
|
if (start_res < 0) {
|
|
|
|
debug_uavcan(1, "UAVCAN: node start problem, error %d\n\r", start_res);
|
|
|
|
return;
|
|
|
|
}
|
2018-10-12 05:15:19 -03:00
|
|
|
|
2018-11-26 23:27:20 -04:00
|
|
|
//Start Servers
|
2018-09-28 12:32:02 -03:00
|
|
|
#ifdef HAS_UAVCAN_SERVERS
|
2018-11-26 23:27:20 -04:00
|
|
|
_servers.init(*_node);
|
2018-09-28 12:32:02 -03:00
|
|
|
#endif
|
2018-07-18 06:26:48 -03:00
|
|
|
// Roundup all subscribers from supported drivers
|
|
|
|
AP_GPS_UAVCAN::subscribe_msgs(this);
|
|
|
|
AP_Compass_UAVCAN::subscribe_msgs(this);
|
|
|
|
AP_Baro_UAVCAN::subscribe_msgs(this);
|
|
|
|
AP_BattMonitor_UAVCAN::subscribe_msgs(this);
|
2018-09-05 03:46:12 -03:00
|
|
|
AP_Airspeed_UAVCAN::subscribe_msgs(this);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-18 00:08:08 -03:00
|
|
|
act_out_array[driver_index] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*_node);
|
2018-02-27 20:02:09 -04:00
|
|
|
act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
|
|
|
|
act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-07-18 00:08:08 -03:00
|
|
|
esc_raw[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*_node);
|
2018-02-27 20:02:09 -04:00
|
|
|
esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
|
|
|
|
esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
|
2017-11-15 10:36:14 -04:00
|
|
|
|
2018-07-18 00:08:08 -03:00
|
|
|
rgb_led[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*_node);
|
2018-02-27 20:02:09 -04:00
|
|
|
rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
|
|
|
rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
_led_conf.devices_count = 0;
|
2018-11-16 05:12:07 -04:00
|
|
|
if (enable_filters) {
|
|
|
|
configureCanAcceptanceFilters(*_node);
|
|
|
|
}
|
2018-07-22 02:07:51 -03:00
|
|
|
|
2018-03-09 04:31:17 -04:00
|
|
|
/*
|
|
|
|
* Informing other nodes that we're ready to work.
|
|
|
|
* Default mode is INITIALIZING.
|
|
|
|
*/
|
2018-07-18 00:08:08 -03:00
|
|
|
_node->setModeOperational();
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
// Spin node for device discovery
|
|
|
|
_node->spin(uavcan::MonotonicDuration::fromMSec(5000));
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
|
|
|
|
_node->setModeOfflineAndPublish();
|
|
|
|
debug_uavcan(1, "UAVCAN: couldn't create thread\n\r");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
_initialized = true;
|
|
|
|
debug_uavcan(2, "UAVCAN: init done\n\r");
|
2017-04-02 11:55:40 -03:00
|
|
|
}
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
void AP_UAVCAN::loop(void)
|
2017-04-02 11:55:40 -03:00
|
|
|
{
|
2018-07-20 10:46:29 -03:00
|
|
|
while (true) {
|
|
|
|
if (!_initialized) {
|
|
|
|
hal.scheduler->delay_microseconds(1000);
|
|
|
|
continue;
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
const int error = _node->spin(uavcan::MonotonicDuration::fromMSec(1));
|
|
|
|
|
|
|
|
if (error < 0) {
|
|
|
|
hal.scheduler->delay_microseconds(100);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (_SRV_armed) {
|
|
|
|
bool sent_servos = false;
|
|
|
|
|
|
|
|
if (_servo_bm > 0) {
|
|
|
|
// if we have any Servos in bitmask
|
|
|
|
uint32_t now = AP_HAL::micros();
|
|
|
|
const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
|
|
|
|
if (now - _SRV_last_send_us >= servo_period_us) {
|
|
|
|
_SRV_last_send_us = now;
|
|
|
|
SRV_send_actuator();
|
|
|
|
sent_servos = true;
|
|
|
|
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
|
|
|
_SRV_conf[i].servo_pending = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// if we have any ESC's in bitmask
|
|
|
|
if (_esc_bm > 0 && !sent_servos) {
|
|
|
|
SRV_send_esc();
|
|
|
|
}
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
|
|
|
_SRV_conf[i].esc_pending = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
led_out_send();
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
}
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
|
|
|
|
///// SRV output /////
|
|
|
|
|
|
|
|
void AP_UAVCAN::SRV_send_actuator(void)
|
2017-04-02 11:55:40 -03:00
|
|
|
{
|
2018-02-08 19:42:58 -04:00
|
|
|
uint8_t starting_servo = 0;
|
|
|
|
bool repeat_send;
|
2017-05-06 06:13:44 -03:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
WITH_SEMAPHORE(SRV_sem);
|
2018-07-20 10:46:29 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
do {
|
|
|
|
repeat_send = false;
|
|
|
|
uavcan::equipment::actuator::ArrayCommand msg;
|
2017-05-06 06:13:44 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
uint8_t i;
|
|
|
|
// UAVCAN can hold maximum of 15 commands in one frame
|
2018-03-09 11:36:21 -04:00
|
|
|
for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) {
|
2018-02-08 19:42:58 -04:00
|
|
|
uavcan::equipment::actuator::Command cmd;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
/*
|
|
|
|
* Servo output uses a range of 1000-2000 PWM for scaling.
|
|
|
|
* This converts output PWM from [1000:2000] range to [-1:1] range that
|
|
|
|
* is passed to servo as unitless type via UAVCAN.
|
|
|
|
* This approach allows for MIN/TRIM/MAX values to be used fully on
|
|
|
|
* autopilot side and for servo it should have the setup to provide maximum
|
|
|
|
* physically possible throws at [-1:1] limits.
|
|
|
|
*/
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-05-24 07:23:00 -03:00
|
|
|
if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
|
2018-02-08 19:42:58 -04:00
|
|
|
cmd.actuator_id = starting_servo + 1;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
// TODO: other types
|
|
|
|
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
// TODO: failsafe, safety
|
2018-03-09 11:36:21 -04:00
|
|
|
cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
msg.commands.push_back(cmd);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
i++;
|
|
|
|
}
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
if (i > 0) {
|
2018-02-27 20:02:09 -04:00
|
|
|
act_out_array[_driver_index]->broadcast(msg);
|
2017-04-17 04:28:38 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
if (i == 15) {
|
|
|
|
repeat_send = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} while (repeat_send);
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 11:36:21 -04:00
|
|
|
void AP_UAVCAN::SRV_send_esc(void)
|
2018-02-08 19:42:58 -04:00
|
|
|
{
|
|
|
|
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
|
|
|
|
uavcan::equipment::esc::RawCommand esc_msg;
|
2017-04-17 04:28:38 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
uint8_t active_esc_num = 0, max_esc_num = 0;
|
|
|
|
uint8_t k = 0;
|
2017-04-17 04:28:38 -03:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
WITH_SEMAPHORE(SRV_sem);
|
2018-07-20 10:46:29 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
// find out how many esc we have enabled and if they are active at all
|
2018-03-09 11:36:21 -04:00
|
|
|
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
|
2018-02-08 19:42:58 -04:00
|
|
|
if ((((uint32_t) 1) << i) & _esc_bm) {
|
|
|
|
max_esc_num = i + 1;
|
2018-05-24 07:23:00 -03:00
|
|
|
if (_SRV_conf[i].esc_pending) {
|
2018-02-08 19:42:58 -04:00
|
|
|
active_esc_num++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
// if at least one is active (update) we need to send to all
|
|
|
|
if (active_esc_num > 0) {
|
|
|
|
k = 0;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
for (uint8_t i = 0; i < max_esc_num && k < 20; i++) {
|
|
|
|
if ((((uint32_t) 1) << i) & _esc_bm) {
|
|
|
|
// TODO: ESC negative scaling for reverse thrust and reverse rotation
|
2018-03-09 11:36:21 -04:00
|
|
|
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
scaled = constrain_float(scaled, 0, cmd_max);
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
esc_msg.cmd.push_back(static_cast<int>(scaled));
|
|
|
|
} else {
|
|
|
|
esc_msg.cmd.push_back(static_cast<unsigned>(0));
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-08 19:42:58 -04:00
|
|
|
k++;
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-02-27 20:02:09 -04:00
|
|
|
esc_raw[_driver_index]->broadcast(esc_msg);
|
2018-02-08 19:42:58 -04:00
|
|
|
}
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
void AP_UAVCAN::SRV_push_servos()
|
2018-02-08 19:42:58 -04:00
|
|
|
{
|
2018-10-11 20:35:04 -03:00
|
|
|
WITH_SEMAPHORE(SRV_sem);
|
2018-02-08 19:42:58 -04:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
|
|
// Check if this channels has any function assigned
|
|
|
|
if (SRV_Channels::channel_function(i)) {
|
|
|
|
_SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm();
|
|
|
|
_SRV_conf[i].esc_pending = true;
|
|
|
|
_SRV_conf[i].servo_pending = true;
|
2018-02-27 20:02:09 -04:00
|
|
|
}
|
2018-07-20 10:46:29 -03:00
|
|
|
}
|
2018-02-08 19:42:58 -04:00
|
|
|
|
2018-08-08 07:58:10 -03:00
|
|
|
_SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
|
2018-07-20 10:46:29 -03:00
|
|
|
}
|
2018-02-08 19:46:57 -04:00
|
|
|
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
///// LED /////
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
void AP_UAVCAN::led_out_send()
|
|
|
|
{
|
|
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
|
|
|
|
if ((now - _led_conf.last_update) < LED_DELAY_US) {
|
|
|
|
return;
|
2018-01-17 03:18:00 -04:00
|
|
|
}
|
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
uavcan::equipment::indication::LightsCommand msg;
|
|
|
|
{
|
|
|
|
WITH_SEMAPHORE(_led_out_sem);
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
if (_led_conf.devices_count == 0) {
|
|
|
|
return;
|
|
|
|
}
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
uavcan::equipment::indication::SingleLightCommand cmd;
|
2018-07-18 00:08:08 -03:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
|
|
|
|
cmd.light_id =_led_conf.devices[i].led_index;
|
|
|
|
cmd.color.red = _led_conf.devices[i].red >> 3;
|
|
|
|
cmd.color.green = _led_conf.devices[i].green >> 2;
|
|
|
|
cmd.color.blue = _led_conf.devices[i].blue >> 3;
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
msg.commands.push_back(cmd);
|
|
|
|
}
|
2018-01-17 03:18:00 -04:00
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
rgb_led[_driver_index]->broadcast(msg);
|
|
|
|
_led_conf.last_update = now;
|
2017-04-02 11:55:40 -03:00
|
|
|
}
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue)
|
2017-04-02 11:55:40 -03:00
|
|
|
{
|
2018-07-20 10:46:29 -03:00
|
|
|
if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) {
|
|
|
|
return false;
|
|
|
|
}
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
WITH_SEMAPHORE(_led_out_sem);
|
2018-03-16 00:19:17 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// check if a device instance exists. if so, break so the instance index is remembered
|
|
|
|
uint8_t instance = 0;
|
|
|
|
for (; instance < _led_conf.devices_count; instance++) {
|
|
|
|
if (_led_conf.devices[instance].led_index == led_index) {
|
|
|
|
break;
|
2018-03-16 00:19:17 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// load into the correct instance.
|
|
|
|
// if an existing instance was found in above for loop search,
|
|
|
|
// then instance value is < _led_conf.devices_count.
|
|
|
|
// otherwise a new one was just found so we increment the count.
|
|
|
|
// Either way, the correct instance is the current value of instance
|
|
|
|
_led_conf.devices[instance].led_index = led_index;
|
|
|
|
_led_conf.devices[instance].red = red;
|
|
|
|
_led_conf.devices[instance].green = green;
|
|
|
|
_led_conf.devices[instance].blue = blue;
|
2018-07-18 00:08:08 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
if (instance == _led_conf.devices_count) {
|
|
|
|
_led_conf.devices_count++;
|
2018-03-16 00:19:17 -03:00
|
|
|
}
|
2018-07-20 10:46:29 -03:00
|
|
|
|
|
|
|
return true;
|
2018-03-16 00:19:17 -03:00
|
|
|
}
|
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
#endif // HAL_WITH_UAVCAN
|