Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/Tools/AP_Periph/can.cpp

1901 lines
65 KiB
C++
Raw Blame History

/*
This program 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.
This program 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/>.
*/
/*
AP_Periph can support
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include "AP_Periph.h"
#include <stdio.h>
#include <drivers/stm32/canard_stm32.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Common/AP_FWVersion.h>
#include <dronecan_msgs.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <hal.h>
#include <AP_HAL_ChibiOS/CANIface.h>
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL_SITL/CANSocketIface.h>
#include <AP_HAL_SITL/AP_HAL_SITL.h>
#endif
#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES))-1U)
#include "i2c.h"
#include <utility>
#if HAL_NUM_CAN_IFACES >= 2
#include <AP_CANManager/AP_CANSensor.h>
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
extern const HAL_SITL &hal;
#else
extern const AP_HAL::HAL &hal;
#endif
extern AP_Periph_FW periph;
#ifndef HAL_PERIPH_LOOP_DELAY_US
// delay between can loop updates. This needs to be longer on F4
#if defined(STM32H7)
#define HAL_PERIPH_LOOP_DELAY_US 64
#else
#define HAL_PERIPH_LOOP_DELAY_US 1024
#endif
#endif
// timeout all frames at 1s
#define CAN_FRAME_TIMEOUT 1000000ULL
#define DEBUG_PKTS 0
#if HAL_PERIPH_CAN_MIRROR
#ifndef HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE
#define HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE 64
#endif
#endif //HAL_PERIPH_CAN_MIRROR
#ifndef HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF
// When enabled, can_printf() strings longer than the droneCAN max text length (90 chars)
// are split into multiple packets instead of truncating the string. This is
// especially helpful with HAL_GCS_ENABLED where libraries use the mavlink
// send_text() method where we support strings up to 256 chars by splitting them
// up into multiple 50 char mavlink packets.
#define HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF (BOARD_FLASH_SIZE >= 1024)
#endif
static struct instance_t {
uint8_t index;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
AP_HAL::CANIface* iface;
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HALSITL::CANIface* iface;
#endif
#if HAL_PERIPH_CAN_MIRROR
#if HAL_NUM_CAN_IFACES < 2
#error "Can't use HAL_PERIPH_CAN_MIRROR if there are not at least 2 HAL_NUM_CAN_IFACES"
#endif
ObjectBuffer<AP_HAL::CANFrame> *mirror_queue;
uint8_t mirror_fail_count;
#endif // HAL_PERIPH_CAN_MIRROR
} instances[HAL_NUM_CAN_IFACES];
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) && (HAL_NUM_CAN_IFACES >= 2)
static ioline_t can_term_lines[] = {
HAL_GPIO_PIN_TERMCAN1
#if HAL_NUM_CAN_IFACES > 2
#ifdef HAL_GPIO_PIN_TERMCAN2
,HAL_GPIO_PIN_TERMCAN2
#else
#error "Only one Can Terminator defined with over two CAN Ifaces"
#endif
#endif
#if HAL_NUM_CAN_IFACES > 2
#ifdef HAL_GPIO_PIN_TERMCAN3
,HAL_GPIO_PIN_TERMCAN3
#else
#error "Only two Can Terminator defined with three CAN Ifaces"
#endif
#endif
};
#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1)
#ifndef HAL_CAN_DEFAULT_NODE_ID
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID
#endif
uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID;
#ifndef AP_PERIPH_PROBE_CONTINUOUS
#define AP_PERIPH_PROBE_CONTINUOUS 0
#endif
#ifndef AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
#define AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz 1
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
ChibiOS::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
#endif
#if AP_CAN_SLCAN_ENABLED
SLCAN::CANIface AP_Periph_FW::slcan_interface;
#endif
#ifdef EXT_FLASH_SIZE_MB
static_assert(EXT_FLASH_SIZE_MB == 0, "DroneCAN bootloader cannot support external flash");
#endif
/*
* Node status variables
*/
static uavcan_protocol_NodeStatus node_status;
#if HAL_ENABLE_SENDING_STATS
static dronecan_protocol_Stats protocol_stats;
#endif
/**
* Returns a pseudo random integer in a given range
*/
static uint16_t get_random_range(uint16_t range)
{
return get_random16() % range;
}
/*
get cpu unique ID
*/
static void readUniqueID(uint8_t* out_uid)
{
uint8_t len = sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data);
memset(out_uid, 0, len);
hal.util->get_system_id_unformatted(out_uid, len);
}
/*
handle a GET_NODE_INFO request
*/
void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance,
CanardRxTransfer* transfer)
{
uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {};
uavcan_protocol_GetNodeInfoResponse pkt {};
node_status.uptime_sec = AP_HAL::millis() / 1000U;
pkt.status = node_status;
pkt.software_version.major = AP::fwversion().major;
pkt.software_version.minor = AP::fwversion().minor;
pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC;
pkt.software_version.vcs_commit = app_descriptor.git_hash;
uint32_t *crc = (uint32_t *)&pkt.software_version.image_crc;
crc[0] = app_descriptor.image_crc1;
crc[1] = app_descriptor.image_crc2;
readUniqueID(pkt.hardware_version.unique_id);
// use hw major/minor for APJ_BOARD_ID so we know what fw is
// compatible with this hardware
pkt.hardware_version.major = APJ_BOARD_ID >> 8;
pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF;
if (g.serial_number > 0) {
hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)g.serial_number);
} else {
hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME);
}
pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data));
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout());
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
UAVCAN_PROTOCOL_GETNODEINFO_ID,
&buffer[0],
total_size);
}
/*
handle parameter GetSet request
*/
void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
// param fetch all can take a long time, so pat watchdog
stm32_watchdog_pat();
uavcan_protocol_param_GetSetRequest req;
if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) {
return;
}
uavcan_protocol_param_GetSetResponse pkt {};
AP_Param *vp;
enum ap_var_type ptype;
if (req.name.len != 0 && req.name.len > AP_MAX_NAME_SIZE) {
vp = nullptr;
} else if (req.name.len != 0 && req.name.len <= AP_MAX_NAME_SIZE) {
memcpy((char *)pkt.name.data, (char *)req.name.data, req.name.len);
vp = AP_Param::find((char *)pkt.name.data, &ptype);
} else {
AP_Param::ParamToken token {};
vp = AP_Param::find_by_index(req.index, &ptype, &token);
if (vp != nullptr) {
vp->copy_name_token(token, (char *)pkt.name.data, AP_MAX_NAME_SIZE+1, true);
}
}
if (vp != nullptr && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) {
// param set
switch (ptype) {
case AP_PARAM_INT8:
if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) {
return;
}
((AP_Int8 *)vp)->set_and_save_ifchanged(req.value.integer_value);
break;
case AP_PARAM_INT16:
if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) {
return;
}
((AP_Int16 *)vp)->set_and_save_ifchanged(req.value.integer_value);
break;
case AP_PARAM_INT32:
if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) {
return;
}
((AP_Int32 *)vp)->set_and_save_ifchanged(req.value.integer_value);
break;
case AP_PARAM_FLOAT:
if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) {
return;
}
((AP_Float *)vp)->set_and_save_ifchanged(req.value.real_value);
break;
default:
return;
}
}
if (vp != nullptr) {
switch (ptype) {
case AP_PARAM_INT8:
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
pkt.value.integer_value = ((AP_Int8 *)vp)->get();
break;
case AP_PARAM_INT16:
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
pkt.value.integer_value = ((AP_Int16 *)vp)->get();
break;
case AP_PARAM_INT32:
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
pkt.value.integer_value = ((AP_Int32 *)vp)->get();
break;
case AP_PARAM_FLOAT:
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
pkt.value.real_value = ((AP_Float *)vp)->get();
break;
default:
return;
}
pkt.name.len = strnlen((char *)pkt.name.data, sizeof(pkt.name.data));
}
uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {};
uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout());
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_GETSET_ID,
&buffer[0],
total_size);
}
/*
handle parameter executeopcode request
*/
void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
uavcan_protocol_param_ExecuteOpcodeRequest req;
if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) {
return;
}
if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) {
StorageManager::erase();
AP_Param::erase_all();
AP_Param::load_all();
AP_Param::setup_sketch_defaults();
#ifdef HAL_PERIPH_ENABLE_GPS
AP_Param::setup_object_defaults(&gps, gps.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY
AP_Param::setup_object_defaults(&battery, battery_lib.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_MAG
AP_Param::setup_object_defaults(&compass, compass.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_BARO
AP_Param::setup_object_defaults(&baro, baro.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
AP_Param::setup_object_defaults(&airspeed, airspeed.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
AP_Param::setup_object_defaults(&rangefinder, rangefinder.var_info);
#endif
}
uavcan_protocol_param_ExecuteOpcodeResponse pkt {};
pkt.ok = true;
uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {};
uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout());
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID,
&buffer[0],
total_size);
}
void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
#if HAL_RAM_RESERVE_START >= 256
// setup information on firmware request at start of ram
auto *comms = (struct app_bootloader_comms *)HAL_RAM0_START;
if (comms->magic != APP_BOOTLOADER_COMMS_MAGIC) {
memset(comms, 0, sizeof(*comms));
}
comms->magic = APP_BOOTLOADER_COMMS_MAGIC;
uavcan_protocol_file_BeginFirmwareUpdateRequest req;
if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) {
return;
}
comms->server_node_id = req.source_node_id;
if (comms->server_node_id == 0) {
comms->server_node_id = transfer->source_node_id;
}
memcpy(comms->path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len);
comms->my_node_id = canardGetLocalNodeID(canard_instance);
uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE] {};
uavcan_protocol_file_BeginFirmwareUpdateResponse reply {};
reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK;
uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout());
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID,
&buffer[0],
total_size);
uint8_t count = 50;
while (count--) {
processTx();
hal.scheduler->delay(1);
}
#endif
// instant reboot, with backup register used to give bootloader
// the node_id
prepare_reboot();
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(canard_instance)));
NVIC_SystemReset();
#endif
}
void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
// Rule C - updating the randomized time interval
dronecan.send_next_node_id_allocation_request_at_ms =
AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
{
printf("Allocation request from another allocatee\n");
dronecan.node_id_allocation_unique_id_offset = 0;
return;
}
// Copying the unique ID from the message
uavcan_protocol_dynamic_node_id_Allocation msg;
uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg);
// Obtaining the local unique ID
uint8_t my_unique_id[sizeof(msg.unique_id.data)];
readUniqueID(my_unique_id);
// Matching the received UID against the local one
if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) {
printf("Mismatching allocation response\n");
dronecan.node_id_allocation_unique_id_offset = 0;
return; // No match, return
}
if (msg.unique_id.len < sizeof(msg.unique_id.data)) {
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
dronecan.node_id_allocation_unique_id_offset = msg.unique_id.len;
dronecan.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
printf("Matching allocation response: %d\n", msg.unique_id.len);
} else {
// Allocation complete - copying the allocated node ID from the message
canardSetLocalNodeID(canard_instance, msg.node_id);
printf("IF%d Node ID allocated: %d\n", dronecan.dna_interface, msg.node_id);
#if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE
if (g.gps_mb_only_can_port) {
// we need to assign the unallocated port to be used for Moving Baseline only
gps_mb_can_port = (dronecan.dna_interface+1)%HAL_NUM_CAN_IFACES;
if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) {
// copy node id from the primary iface
canardSetLocalNodeID(&dronecan.canard, msg.node_id);
#ifdef HAL_GPIO_PIN_TERMCAN1
// also terminate the line as we don't have any other device on this port
palWriteLine(can_term_lines[gps_mb_can_port], 1);
#endif
}
}
#endif
}
}
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
static uint8_t safety_state;
/*
handle SafetyState
*/
void AP_Periph_FW::handle_safety_state(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
ardupilot_indication_SafetyState req;
if (ardupilot_indication_SafetyState_decode(transfer, &req)) {
return;
}
safety_state = req.status;
#if AP_PERIPH_SAFETY_SWITCH_ENABLED
rcout_handle_safety_state(safety_state);
#endif
}
#endif // HAL_GPIO_PIN_SAFE_LED
/*
handle ArmingStatus
*/
void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
uavcan_equipment_safety_ArmingStatus req;
if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &req)) {
return;
}
hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED);
}
#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY)
void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
{
#ifdef HAL_PERIPH_ENABLE_NOTIFY
notify.handle_rgb(red, green, blue);
#ifdef HAL_PERIPH_ENABLE_RC_OUT
rcout_has_new_data_to_update = true;
#endif // HAL_PERIPH_ENABLE_RC_OUT
#endif // HAL_PERIPH_ENABLE_NOTIFY
#ifdef HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY
hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, -1, red, green, blue);
hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY);
#endif // HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY
#ifdef HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY
{
const uint8_t i2c_address = 0x38;
static AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
if (!dev) {
dev = std::move(hal.i2c_mgr->get_device(0, i2c_address));
}
WITH_SEMAPHORE(dev->get_semaphore());
dev->set_retries(0);
uint8_t v = 0x3f; // enable LED
dev->transfer(&v, 1, nullptr, 0);
v = 0x40 | red >> 3; // red
dev->transfer(&v, 1, nullptr, 0);
v = 0x60 | green >> 3; // green
dev->transfer(&v, 1, nullptr, 0);
v = 0x80 | blue >> 3; // blue
dev->transfer(&v, 1, nullptr, 0);
}
#endif // HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY
#ifdef HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY
{
const uint8_t i2c_address = 0x38;
static AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
if (!dev) {
dev = std::move(hal.i2c_mgr->get_device(0, i2c_address));
}
WITH_SEMAPHORE(dev->get_semaphore());
dev->set_retries(0);
uint8_t v = 0x3f; // enable LED
dev->transfer(&v, 1, nullptr, 0);
v = 0x40 | blue >> 3; // blue
dev->transfer(&v, 1, nullptr, 0);
v = 0x60 | green >> 3; // green
dev->transfer(&v, 1, nullptr, 0);
v = 0x80 | red >> 3; // red
dev->transfer(&v, 1, nullptr, 0);
}
#endif // HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY
#ifdef HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY
{
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register
#define TOSHIBA_LED_ENABLE 0x04 // enable register
#define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address
static AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_toshiba;
if (!dev_toshiba) {
dev_toshiba = std::move(hal.i2c_mgr->get_device(0, TOSHIBA_LED_I2C_ADDR));
}
WITH_SEMAPHORE(dev_toshiba->get_semaphore());
dev_toshiba->set_retries(0); // use 0 because this is running on main thread.
// enable the led
dev_toshiba->write_register(TOSHIBA_LED_ENABLE, 0x03);
/* 4-bit for each color */
uint8_t val[4] = {
TOSHIBA_LED_PWM0,
(uint8_t)(blue >> 4),
(uint8_t)(green / 16),
(uint8_t)(red / 16)
};
dev_toshiba->transfer(val, sizeof(val), nullptr, 0);
}
#endif // HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY
}
/*
handle lightscommand
*/
void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
uavcan_equipment_indication_LightsCommand req;
if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) {
return;
}
for (uint8_t i=0; i<req.commands.len; i++) {
uavcan_equipment_indication_SingleLightCommand &cmd = req.commands.data[i];
// to get the right color proportions we scale the green so that is uses the
// same number of bits as red and blue
uint8_t red = cmd.color.red<<3U;
uint8_t green = (cmd.color.green>>1U)<<3U;
uint8_t blue = cmd.color.blue<<3U;
#ifdef HAL_PERIPH_ENABLE_NOTIFY
const int8_t brightness = notify.get_rgb_led_brightness_percent();
#elif defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY)
const int8_t brightness = g.led_brightness;
#endif
if (brightness != 100 && brightness >= 0) {
const float scale = brightness * 0.01;
red = constrain_int16(red * scale, 0, 255);
green = constrain_int16(green * scale, 0, 255);
blue = constrain_int16(blue * scale, 0, 255);
}
set_rgb_led(red, green, blue);
}
}
#endif // AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY
#ifdef HAL_PERIPH_ENABLE_RC_OUT
void AP_Periph_FW::handle_esc_rawcommand(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
uavcan_equipment_esc_RawCommand cmd;
if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) {
return;
}
rcout_esc(cmd.cmd.data, cmd.cmd.len);
// Update internal copy for disabling output to ESC when CAN packets are lost
last_esc_num_channels = cmd.cmd.len;
last_esc_raw_command_ms = AP_HAL::millis();
}
void AP_Periph_FW::handle_act_command(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
uavcan_equipment_actuator_ArrayCommand cmd;
if (uavcan_equipment_actuator_ArrayCommand_decode(transfer, &cmd)) {
return;
}
for (uint8_t i=0; i < cmd.commands.len; i++) {
const auto &c = cmd.commands.data[i];
switch (c.command_type) {
case UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS:
rcout_srv_unitless(c.actuator_id, c.command_value);
break;
case UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM:
rcout_srv_PWM(c.actuator_id, c.command_value);
break;
}
}
}
#endif // HAL_PERIPH_ENABLE_RC_OUT
#if defined(HAL_PERIPH_ENABLE_NOTIFY)
void AP_Periph_FW::handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer)
{
ardupilot_indication_NotifyState msg;
if (ardupilot_indication_NotifyState_decode(transfer, &msg)) {
return;
}
if (msg.aux_data.len == 2 && msg.aux_data_type == ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES) {
uint16_t tmp = 0;
memcpy(&tmp, msg.aux_data.data, sizeof(tmp));
yaw_earth = radians((float)tmp * 0.01f);
}
vehicle_state = msg.vehicle_state;
last_vehicle_state = AP_HAL::millis();
}
#endif // HAL_PERIPH_ENABLE_NOTIFY
#ifdef HAL_GPIO_PIN_SAFE_LED
/*
update safety LED
*/
void AP_Periph_FW::can_safety_LED_update(void)
{
static uint32_t last_update_ms;
switch (safety_state) {
case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF:
palWriteLine(HAL_GPIO_PIN_SAFE_LED, SAFE_LED_ON);
break;
case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON: {
uint32_t now = AP_HAL::millis();
if (now - last_update_ms > 100) {
last_update_ms = now;
static uint8_t led_counter;
const uint16_t led_pattern = 0x5500;
led_counter = (led_counter+1) % 16;
palWriteLine(HAL_GPIO_PIN_SAFE_LED, (led_pattern & (1U << led_counter))?!SAFE_LED_ON:SAFE_LED_ON);
}
break;
}
default:
palWriteLine(HAL_GPIO_PIN_SAFE_LED, !SAFE_LED_ON);
break;
}
}
#endif // HAL_GPIO_PIN_SAFE_LED
#ifdef HAL_GPIO_PIN_SAFE_BUTTON
#ifndef HAL_SAFE_BUTTON_ON
#define HAL_SAFE_BUTTON_ON 1
#endif
/*
update safety button
*/
void AP_Periph_FW::can_safety_button_update(void)
{
static uint32_t last_update_ms;
static uint8_t counter;
uint32_t now = AP_HAL::millis();
// send at 10Hz when pressed
if (palReadLine(HAL_GPIO_PIN_SAFE_BUTTON) != HAL_SAFE_BUTTON_ON) {
counter = 0;
return;
}
if (now - last_update_ms < 100) {
return;
}
if (counter < 255) {
counter++;
}
last_update_ms = now;
ardupilot_indication_Button pkt {};
pkt.button = ARDUPILOT_INDICATION_BUTTON_BUTTON_SAFETY;
pkt.press_time = counter;
uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {};
uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !canfdout());
canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE,
ARDUPILOT_INDICATION_BUTTON_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
#endif // HAL_GPIO_PIN_SAFE_BUTTON
/**
* This callback is invoked by the library when a new message or request or response is received.
*/
void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance,
CanardRxTransfer* transfer)
{
#ifdef HAL_GPIO_PIN_LED_CAN1
palToggleLine(HAL_GPIO_PIN_LED_CAN1);
#endif
#if HAL_CANFD_SUPPORTED
// enable tao for decoding when not on CANFD
transfer->tao = !transfer->canfd;
#endif
/*
* Dynamic node ID allocation protocol.
* Taking this branch only if we don't have a node ID, ignoring otherwise.
*/
if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID) {
if (transfer->transfer_type == CanardTransferTypeBroadcast &&
transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
handle_allocation_response(canard_instance, transfer);
}
return;
}
switch (transfer->data_type_id) {
case UAVCAN_PROTOCOL_GETNODEINFO_ID:
handle_get_node_info(canard_instance, transfer);
break;
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
handle_begin_firmware_update(canard_instance, transfer);
break;
case UAVCAN_PROTOCOL_RESTARTNODE_ID:
printf("RestartNode\n");
hal.scheduler->delay(10);
prepare_reboot();
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
NVIC_SystemReset();
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HAL_SITL::actually_reboot();
#endif
break;
case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
handle_param_getset(canard_instance, transfer);
break;
case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID:
handle_param_executeopcode(canard_instance, transfer);
break;
#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY)
case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID:
handle_beep_command(canard_instance, transfer);
break;
#endif
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
handle_safety_state(canard_instance, transfer);
break;
#endif
case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID:
handle_arming_status(canard_instance, transfer);
break;
#ifdef HAL_PERIPH_ENABLE_GPS
case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
handle_RTCMStream(canard_instance, transfer);
break;
#if GPS_MOVING_BASELINE
case ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID:
handle_MovingBaselineData(canard_instance, transfer);
break;
#endif
#endif // HAL_PERIPH_ENABLE_GPS
#if AP_UART_MONITOR_ENABLED
case UAVCAN_TUNNEL_TARGETTED_ID:
handle_tunnel_Targetted(canard_instance, transfer);
break;
#endif
#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY)
case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID:
handle_lightscommand(canard_instance, transfer);
break;
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
handle_esc_rawcommand(canard_instance, transfer);
break;
case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
handle_act_command(canard_instance, transfer);
break;
#endif
#ifdef HAL_PERIPH_ENABLE_NOTIFY
case ARDUPILOT_INDICATION_NOTIFYSTATE_ID:
handle_notify_state(canard_instance, transfer);
break;
#endif
#ifdef HAL_PERIPH_ENABLE_RELAY
case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID:
handle_hardpoint_command(canard_instance, transfer);
break;
#endif
}
}
/**
* This callback is invoked by the library when a new message or request or response is received.
*/
static void onTransferReceived_trampoline(CanardInstance* canard_instance,
CanardRxTransfer* transfer)
{
AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference;
fw->onTransferReceived(canard_instance, transfer);
}
/**
* This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received
* by the local node.
* If the callback returns true, the library will receive the transfer.
* If the callback returns false, the library will ignore the transfer.
* All transfers that are addressed to other nodes are always ignored.
*/
bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance,
uint64_t* out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id)
{
(void)source_node_id;
if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID)
{
/*
* If we're in the process of allocation of dynamic node ID, accept only relevant transfers.
*/
if ((transfer_type == CanardTransferTypeBroadcast) &&
(data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID))
{
*out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
return true;
}
return false;
}
switch (data_type_id) {
case UAVCAN_PROTOCOL_GETNODEINFO_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
return true;
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE;
return true;
case UAVCAN_PROTOCOL_RESTARTNODE_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
return true;
case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
return true;
case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
return true;
#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY)
case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE;
return true;
#endif
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
*out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE;
return true;
#endif
case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_SIGNATURE;
return true;
#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY)
case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_SIGNATURE;
return true;
#endif
#ifdef HAL_PERIPH_ENABLE_GPS
case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE;
return true;
#if GPS_MOVING_BASELINE
case ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID:
*out_data_type_signature = ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE;
return true;
#endif
#endif // HAL_PERIPH_ENABLE_GPS
#if AP_UART_MONITOR_ENABLED
case UAVCAN_TUNNEL_TARGETTED_ID:
*out_data_type_signature = UAVCAN_TUNNEL_TARGETTED_SIGNATURE;
return true;
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
return true;
case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE;
return true;
#endif
#if defined(HAL_PERIPH_ENABLE_NOTIFY)
case ARDUPILOT_INDICATION_NOTIFYSTATE_ID:
*out_data_type_signature = ARDUPILOT_INDICATION_NOTIFYSTATE_SIGNATURE;
return true;
#endif
#ifdef HAL_PERIPH_ENABLE_RELAY
case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE;
return true;
#endif
default:
break;
}
return false;
}
static bool shouldAcceptTransfer_trampoline(const CanardInstance* canard_instance,
uint64_t* out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id)
{
AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference;
return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id);
}
void AP_Periph_FW::cleanup_stale_transactions(uint64_t timestamp_usec)
{
canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec);
}
uint8_t *AP_Periph_FW::get_tid_ptr(uint32_t transfer_desc)
{
// check head
if (!dronecan.tid_map_head) {
dronecan.tid_map_head = (dronecan_protocol_t::tid_map*)calloc(1, sizeof(dronecan_protocol_t::tid_map));
if (dronecan.tid_map_head == nullptr) {
return nullptr;
}
dronecan.tid_map_head->transfer_desc = transfer_desc;
dronecan.tid_map_head->next = nullptr;
return &dronecan.tid_map_head->tid;
} else if (dronecan.tid_map_head->transfer_desc == transfer_desc) {
return &dronecan.tid_map_head->tid;
}
// search through the list for an existing entry
dronecan_protocol_t::tid_map *tid_map_ptr = dronecan.tid_map_head;
while(tid_map_ptr->next) {
tid_map_ptr = tid_map_ptr->next;
if (tid_map_ptr->transfer_desc == transfer_desc) {
return &tid_map_ptr->tid;
}
}
// create a new entry, if not found
tid_map_ptr->next = (dronecan_protocol_t::tid_map*)calloc(1, sizeof(dronecan_protocol_t::tid_map));
if (tid_map_ptr->next == nullptr) {
return nullptr;
}
tid_map_ptr->next->transfer_desc = transfer_desc;
tid_map_ptr->next->next = nullptr;
return &tid_map_ptr->next->tid;
}
bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
uint16_t data_type_id,
uint8_t priority,
const void* payload,
uint16_t payload_len,
uint8_t iface_mask)
{
const bool is_dna = data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID;
if (!is_dna && canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) {
return false;
}
uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(data_type_signature, data_type_id, 0, CANARD_BROADCAST_NODE_ID));
if (tid_ptr == nullptr) {
return false;
}
// create transfer object
CanardTxTransfer transfer_object = {
.transfer_type = CanardTransferTypeBroadcast,
.data_type_signature = data_type_signature,
.data_type_id = data_type_id,
.inout_transfer_id = tid_ptr,
.priority = priority,
.payload = (uint8_t*)payload,
.payload_len = payload_len,
#if CANARD_ENABLE_CANFD
.canfd = is_dna? false : canfdout(),
#endif
.deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT,
#if CANARD_MULTI_IFACE
.iface_mask = iface_mask==0 ? uint8_t(IFACE_ALL) : iface_mask,
#endif
};
const int16_t res = canardBroadcastObj(&dronecan.canard, &transfer_object);
#if DEBUG_PKTS
if (res < 0) {
can_printf("Tx error %d\n", res);
}
#endif
#if HAL_ENABLE_SENDING_STATS
if (res <= 0) {
protocol_stats.tx_errors++;
} else {
protocol_stats.tx_frames += res;
}
#endif
return res > 0;
}
/*
send a response
*/
bool AP_Periph_FW::canard_respond(CanardInstance* canard_instance,
CanardRxTransfer *transfer,
uint64_t data_type_signature,
uint16_t data_type_id,
const uint8_t *payload,
uint16_t payload_len)
{
CanardTxTransfer transfer_object = {
.transfer_type = CanardTransferTypeResponse,
.data_type_signature = data_type_signature,
.data_type_id = data_type_id,
.inout_transfer_id = &transfer->transfer_id,
.priority = transfer->priority,
.payload = payload,
.payload_len = payload_len,
#if CANARD_ENABLE_CANFD
.canfd = canfdout(),
#endif
.deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT,
#if CANARD_MULTI_IFACE
.iface_mask = IFACE_ALL,
#endif
};
const auto res = canardRequestOrRespondObj(canard_instance,
transfer->source_node_id,
&transfer_object);
#if DEBUG_PKTS
if (res < 0) {
can_printf("Tx error %d\n", res);
}
#endif
#if HAL_ENABLE_SENDING_STATS
if (res <= 0) {
protocol_stats.tx_errors++;
} else {
protocol_stats.tx_frames += res;
}
#endif
return res > 0;
}
void AP_Periph_FW::processTx(void)
{
for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) {
AP_HAL::CANFrame txmsg {};
txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
memcpy(txmsg.data, txf->data, txf->data_len);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
#if HAL_CANFD_SUPPORTED
txmsg.canfd = txf->canfd;
#endif
// push message with 1s timeout
bool sent = true;
const uint64_t now_us = AP_HAL::micros64();
const uint64_t deadline = now_us + 1000000U;
// try sending to all interfaces
for (auto &_ins : instances) {
if (_ins.iface == NULL) {
continue;
}
#if CANARD_MULTI_IFACE
if (!(txf->iface_mask & (1U<<_ins.index))) {
continue;
}
#endif
#if HAL_NUM_CAN_IFACES >= 2
if (can_protocol_cached[_ins.index] != AP_CAN::Protocol::DroneCAN) {
continue;
}
#endif
if (_ins.iface->send(txmsg, deadline, 0) <= 0) {
/*
We were not able to queue the frame for
sending. Only mark the send as failing if the
interface is active. We consider an interface as
active if it has had a successful transmit in the
last 2 seconds
*/
volatile const auto *stats = _ins.iface->get_statistics();
uint64_t last_transmit_us = stats->last_transmit_us;
if (stats == nullptr || AP_HAL::micros64() - last_transmit_us < 2000000UL) {
sent = false;
}
} else {
#if CANARD_MULTI_IFACE
txf->iface_mask &= ~(1U<<_ins.index);
#endif
}
}
if (sent) {
canardPopTxQueue(&dronecan.canard);
dronecan.tx_fail_count = 0;
} else {
// exit and try again later. If we fail 8 times in a row
// then cleanup any stale transfers to keep the queue from
// filling
if (dronecan.tx_fail_count < 8) {
dronecan.tx_fail_count++;
} else {
#if HAL_ENABLE_SENDING_STATS
protocol_stats.tx_errors++;
#endif
dronecan.tx_fail_count = 0;
cleanup_stale_transactions(now_us);
}
break;
}
}
}
#if HAL_ENABLE_SENDING_STATS
void AP_Periph_FW::update_rx_protocol_stats(int16_t res)
{
switch (res) {
case CANARD_OK:
protocol_stats.rx_frames++;
break;
case -CANARD_ERROR_OUT_OF_MEMORY:
protocol_stats.rx_error_oom++;
break;
case -CANARD_ERROR_INTERNAL:
protocol_stats.rx_error_internal++;
break;
case -CANARD_ERROR_RX_INCOMPATIBLE_PACKET:
protocol_stats.rx_ignored_not_wanted++;
break;
case -CANARD_ERROR_RX_WRONG_ADDRESS:
protocol_stats.rx_ignored_wrong_address++;
break;
case -CANARD_ERROR_RX_NOT_WANTED:
protocol_stats.rx_ignored_not_wanted++;
break;
case -CANARD_ERROR_RX_MISSED_START:
protocol_stats.rx_error_missed_start++;
break;
case -CANARD_ERROR_RX_WRONG_TOGGLE:
protocol_stats.rx_error_wrong_toggle++;
break;
case -CANARD_ERROR_RX_UNEXPECTED_TID:
protocol_stats.rx_ignored_unexpected_tid++;
break;
case -CANARD_ERROR_RX_SHORT_FRAME:
protocol_stats.rx_error_short_frame++;
break;
case -CANARD_ERROR_RX_BAD_CRC:
protocol_stats.rx_error_bad_crc++;
break;
default:
// mark all other errors as internal
protocol_stats.rx_error_internal++;
break;
}
}
#endif
void AP_Periph_FW::processRx(void)
{
AP_HAL::CANFrame rxmsg;
for (auto &instance : instances) {
if (instance.iface == NULL) {
continue;
}
#if HAL_NUM_CAN_IFACES >= 2
if (can_protocol_cached[instance.index] != AP_CAN::Protocol::DroneCAN) {
continue;
}
#endif
while (true) {
bool read_select = true;
bool write_select = false;
instance.iface->select(read_select, write_select, nullptr, 0);
if (!read_select) { // No data pending
break;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
if (instance.iface->receive(rxmsg, timestamp, flags) <= 0) {
break;
}
#if HAL_PERIPH_CAN_MIRROR
for (auto &other_instance : instances) {
if (other_instance.mirror_queue == nullptr) { // we aren't mirroring here, or failed on memory
continue;
}
if (other_instance.index == ins.index) { // don't self add
continue;
}
other_instance.mirror_queue->push(rxmsg);
}
#endif // HAL_PERIPH_CAN_MIRROR
rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
#if HAL_CANFD_SUPPORTED
rx_frame.canfd = rxmsg.canfd;
#endif
rx_frame.id = rxmsg.id;
#if CANARD_MULTI_IFACE
rx_frame.iface_id = instance.index;
#endif
const int16_t res = canardHandleRxFrame(&dronecan.canard, &rx_frame, timestamp);
#if HAL_ENABLE_SENDING_STATS
if (res == -CANARD_ERROR_RX_MISSED_START) {
// this might remaining frames from a message that we don't accept, so check
uint64_t dummy_signature;
if (shouldAcceptTransfer(&dronecan.canard,
&dummy_signature,
extractDataType(rx_frame.id),
extractTransferType(rx_frame.id),
1)) { // doesn't matter what we pass here
update_rx_protocol_stats(res);
} else {
protocol_stats.rx_ignored_not_wanted++;
}
} else {
update_rx_protocol_stats(res);
}
#else
(void)res;
#endif
}
}
}
#if HAL_PERIPH_CAN_MIRROR
void AP_Periph_FW::processMirror(void)
{
const uint64_t deadline = AP_HAL::micros64() + 1000000;
for (auto &ins : instances) {
if (ins.iface == nullptr || ins.mirror_queue == nullptr) { // can't send on a null interface
continue;
}
const uint32_t pending = ins.mirror_queue->available();
for (uint32_t i = 0; i < pending; i++) { // limit how long we can loop
AP_HAL::CANFrame txmsg {};
if (!ins.mirror_queue->peek(txmsg)) {
break;
}
if (ins.iface->send(txmsg, deadline, 0) <= 0) {
if (ins.mirror_fail_count < 8) {
ins.mirror_fail_count++;
} else {
ins.mirror_queue->pop();
}
break;
} else {
ins.mirror_fail_count = 0;
ins.mirror_queue->pop();
}
}
}
}
#endif // HAL_PERIPH_CAN_MIRROR
uint16_t AP_Periph_FW::pool_peak_percent()
{
const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&dronecan.canard);
const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks);
return peak_percent;
}
void AP_Periph_FW::node_status_send(void)
{
{
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
node_status.uptime_sec = AP_HAL::millis() / 1000U;
node_status.vendor_specific_status_code = MIN(hal.util->available_memory(), unsigned(UINT16_MAX));
uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !canfdout());
canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
UAVCAN_PROTOCOL_NODESTATUS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
}
#if HAL_ENABLE_SENDING_STATS
if (debug_option_is_set(AP_Periph_FW::DebugOptions::ENABLE_STATS)) {
{
uint8_t buffer[DRONECAN_PROTOCOL_STATS_MAX_SIZE];
uint32_t len = dronecan_protocol_Stats_encode(&protocol_stats, buffer, !canfdout());
canard_broadcast(DRONECAN_PROTOCOL_STATS_SIGNATURE,
DRONECAN_PROTOCOL_STATS_ID,
CANARD_TRANSFER_PRIORITY_LOWEST,
buffer,
len);
}
for (auto &instance : instances) {
uint8_t buffer[DRONECAN_PROTOCOL_CANSTATS_MAX_SIZE];
dronecan_protocol_CanStats can_stats;
const AP_HAL::CANIface::bus_stats_t *bus_stats = instance.iface->get_statistics();
if (bus_stats == nullptr) {
return;
}
can_stats.interface = instance.index;
can_stats.tx_requests = bus_stats->tx_requests;
can_stats.tx_rejected = bus_stats->tx_rejected;
can_stats.tx_overflow = bus_stats->tx_overflow;
can_stats.tx_success = bus_stats->tx_success;
can_stats.tx_timedout = bus_stats->tx_timedout;
can_stats.tx_abort = bus_stats->tx_abort;
can_stats.rx_received = bus_stats->rx_received;
can_stats.rx_overflow = bus_stats->rx_overflow;
can_stats.rx_errors = bus_stats->rx_errors;
can_stats.busoff_errors = bus_stats->num_busoff_err;
uint32_t len = dronecan_protocol_CanStats_encode(&can_stats, buffer, !canfdout());
canard_broadcast(DRONECAN_PROTOCOL_CANSTATS_SIGNATURE,
DRONECAN_PROTOCOL_CANSTATS_ID,
CANARD_TRANSFER_PRIORITY_LOWEST,
buffer,
len);
}
}
#endif
}
/**
* This function is called at 1 Hz rate from the main loop.
*/
void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec)
{
/*
* Purging transfers that are no longer transmitted. This will occasionally free up some memory.
*/
cleanup_stale_transactions(timestamp_usec);
/*
* Printing the memory usage statistics.
*/
{
/*
* The recommended way to establish the minimal size of the memory pool is to stress-test the application and
* record the worst case memory usage.
*/
if (pool_peak_percent() > 70) {
printf("WARNING: ENLARGE MEMORY POOL\n");
}
}
/*
* Transmitting the node status message periodically.
*/
node_status_send();
#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
if (g.flash_bootloader.get()) {
const uint8_t flash_bl = g.flash_bootloader.get();
g.flash_bootloader.set_and_save_ifchanged(0);
if (flash_bl == 42) {
// magic developer value to test watchdog support with main loop lockup
while (true) {
can_printf("entering lockup\n");
hal.scheduler->delay(100);
}
}
if (flash_bl == 43) {
// magic developer value to test watchdog support with hard fault
can_printf("entering fault\n");
void *foo = (void*)0xE000ED38;
typedef void (*fptr)();
fptr gptr = (fptr) (void *) foo;
gptr();
}
EXPECT_DELAY_MS(2000);
hal.scheduler->delay(1000);
AP_HAL::Util::FlashBootloader res = hal.util->flash_bootloader();
switch (res) {
case AP_HAL::Util::FlashBootloader::OK:
can_printf("Flash bootloader OK\n");
break;
case AP_HAL::Util::FlashBootloader::NO_CHANGE:
can_printf("Bootloader unchanged\n");
break;
#if AP_SIGNED_FIRMWARE
case AP_HAL::Util::FlashBootloader::NOT_SIGNED:
can_printf("Bootloader not signed\n");
break;
#endif
default:
can_printf("Flash bootloader FAILED\n");
break;
}
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (hal.run_in_maintenance_mode()) {
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE;
} else
#endif
{
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
}
#if 0
// test code for watchdog reset
if (AP_HAL::millis() > 15000) {
while (true) ;
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if (AP_HAL::millis() > 30000) {
// use RTC to mark that we have been running fine for
// 30s. This is used along with watchdog resets to ensure the
// user has a chance to load a fixed firmware
set_fast_reboot(RTC_BOOT_FWOK);
}
#endif
}
/*
wait for dynamic allocation of node ID
*/
bool AP_Periph_FW::no_iface_finished_dna = true;
bool AP_Periph_FW::can_do_dna()
{
if (canardGetLocalNodeID(&dronecan.canard) != CANARD_BROADCAST_NODE_ID) {
AP_Periph_FW::no_iface_finished_dna = false;
return true;
}
const uint32_t now = AP_HAL::millis();
if (AP_Periph_FW::no_iface_finished_dna) {
printf("Waiting for dynamic node ID allocation %x... (pool %u)\n", IFACE_ALL, pool_peak_percent());
}
dronecan.send_next_node_id_allocation_request_at_ms =
now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
// Structure of the request is documented in the DSDL definition
// See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1];
allocation_request[0] = (uint8_t)(PreferredNodeID << 1U);
if (dronecan.node_id_allocation_unique_id_offset == 0) {
allocation_request[0] |= 1; // First part of unique ID
// set interface to try
dronecan.dna_interface++;
dronecan.dna_interface %= HAL_NUM_CAN_IFACES;
}
uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
readUniqueID(my_unique_id);
static const uint8_t MaxLenOfUniqueIDInRequest = 6;
uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - dronecan.node_id_allocation_unique_id_offset);
if (uid_size > MaxLenOfUniqueIDInRequest) {
uid_size = MaxLenOfUniqueIDInRequest;
}
memmove(&allocation_request[1], &my_unique_id[dronecan.node_id_allocation_unique_id_offset], uid_size);
// Broadcasting the request
canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&allocation_request[0],
(uint16_t) (uid_size + 1));
// Preparing for timeout; if response is received, this value will be updated from the callback.
dronecan.node_id_allocation_unique_id_offset = 0;
return false;
}
void AP_Periph_FW::can_start()
{
node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION;
node_status.uptime_sec = AP_HAL::millis() / 1000U;
if (g.can_node >= 0 && g.can_node < 128) {
PreferredNodeID = g.can_node;
}
#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
g.flash_bootloader.set_and_save_ifchanged(0);
#endif
#if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2
bool has_uavcan_at_1MHz = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
if (g.can_protocol[i] == AP_CAN::Protocol::DroneCAN && g.can_baudrate[i] == 1000000) {
has_uavcan_at_1MHz = true;
}
}
if (!has_uavcan_at_1MHz) {
g.can_protocol[0].set_and_save(uint8_t(AP_CAN::Protocol::DroneCAN));
g.can_baudrate[0].set_and_save(1000000);
}
#endif // HAL_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
#ifdef HAL_GPIO_PIN_GPIO_CAN1_TERM
palWriteLine(HAL_GPIO_PIN_GPIO_CAN1_TERM, g.can_terminate[0]);
#endif
#ifdef HAL_GPIO_PIN_GPIO_CAN2_TERM
palWriteLine(HAL_GPIO_PIN_GPIO_CAN2_TERM, g.can_terminate[1]);
#endif
#ifdef HAL_GPIO_PIN_GPIO_CAN3_TERM
palWriteLine(HAL_GPIO_PIN_GPIO_CAN3_TERM, g.can_terminate[2]);
#endif
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
can_iface_periph[i] = new ChibiOS::CANIface();
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
can_iface_periph[i] = new HALSITL::CANIface();
#endif
instances[i].iface = can_iface_periph[i];
instances[i].index = i;
#if HAL_PERIPH_CAN_MIRROR
if ((g.can_mirror_ports & (1U << i)) != 0) {
instances[i].mirror_queue = new ObjectBuffer<AP_HAL::CANFrame> (HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE);
}
#endif //HAL_PERIPH_CAN_MIRROR
#if HAL_NUM_CAN_IFACES >= 2
can_protocol_cached[i] = g.can_protocol[i];
CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]);
#endif
if (can_iface_periph[i] != nullptr) {
#if HAL_CANFD_SUPPORTED
can_iface_periph[i]->init(g.can_baudrate[i], g.can_fdbaudrate[i]*1000000U, AP_HAL::CANIface::NormalMode);
#else
can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
#endif
}
}
#if AP_CAN_SLCAN_ENABLED
const uint8_t slcan_selected_index = g.can_slcan_cport - 1;
if (slcan_selected_index < HAL_NUM_CAN_IFACES) {
slcan_interface.set_can_iface(can_iface_periph[slcan_selected_index]);
instances[slcan_selected_index].iface = (AP_HAL::CANIface*)&slcan_interface;
// ensure there's a serial port mapped to SLCAN
if (!serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) {
serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000);
}
}
#endif
canardInit(&dronecan.canard, (uint8_t *)dronecan.canard_memory_pool, sizeof(dronecan.canard_memory_pool),
onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, this);
if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) {
canardSetLocalNodeID(&dronecan.canard, PreferredNodeID);
}
}
#ifdef HAL_PERIPH_ENABLE_RC_OUT
#if HAL_WITH_ESC_TELEM
/*
send ESC status packets based on AP_ESC_Telem
*/
void AP_Periph_FW::esc_telem_update()
{
uint32_t mask = esc_telem.get_active_esc_mask();
while (mask != 0) {
int8_t i = __builtin_ffs(mask) - 1;
mask &= ~(1U<<i);
const float nan = nanf("");
uavcan_equipment_esc_Status pkt {};
const auto *channel = SRV_Channels::srv_channel(i);
// try to map the ESC number to a motor number. This is needed
// for when we have multiple CAN nodes, one for each ESC
if (channel == nullptr) {
pkt.esc_index = i;
} else {
const int8_t motor_num = channel->get_motor_num();
pkt.esc_index = motor_num==-1? i:motor_num;
}
if (!esc_telem.get_voltage(i, pkt.voltage)) {
pkt.voltage = nan;
}
if (!esc_telem.get_current(i, pkt.current)) {
pkt.current = nan;
}
int16_t temperature;
if (esc_telem.get_motor_temperature(i, temperature)) {
pkt.temperature = C_TO_KELVIN(temperature*0.01);
} else if (esc_telem.get_temperature(i, temperature)) {
pkt.temperature = C_TO_KELVIN(temperature*0.01);
} else {
pkt.temperature = nan;
}
float rpm;
if (esc_telem.get_raw_rpm(i, rpm)) {
pkt.rpm = rpm;
}
pkt.power_rating_pct = 0;
pkt.error_count = 0;
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
UAVCAN_EQUIPMENT_ESC_STATUS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
}
#endif // HAL_WITH_ESC_TELEM
#ifdef HAL_PERIPH_ENABLE_ESC_APD
void AP_Periph_FW::apd_esc_telem_update()
{
for(uint8_t i = 0; i < ARRAY_SIZE(apd_esc_telem); i++) {
if (apd_esc_telem[i] == nullptr) {
continue;
}
ESC_APD_Telem &esc = *apd_esc_telem[i];
if (esc.update()) {
const ESC_APD_Telem::telem &t = esc.get_telem();
uavcan_equipment_esc_Status pkt {};
static_assert(APD_ESC_INSTANCES <= ARRAY_SIZE(g.esc_number), "There must be an ESC instance number for each APD ESC");
pkt.esc_index = g.esc_number[i];
pkt.voltage = t.voltage;
pkt.current = t.current;
pkt.temperature = t.temperature;
pkt.rpm = t.rpm;
pkt.power_rating_pct = t.power_rating_pct;
pkt.error_count = t.error_count;
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
UAVCAN_EQUIPMENT_ESC_STATUS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
}
}
#endif // HAL_PERIPH_ENABLE_ESC_APD
#endif // HAL_PERIPH_ENABLE_RC_OUT
void AP_Periph_FW::can_update()
{
const uint32_t now = AP_HAL::millis();
const uint32_t led_pattern = 0xAAAA;
const uint32_t led_change_period = 50;
static uint8_t led_idx = 0;
static uint32_t last_led_change;
if ((now - last_led_change > led_change_period) && no_iface_finished_dna) {
// blink LED in recognisable pattern while waiting for DNA
#ifdef HAL_GPIO_PIN_LED
palWriteLine(HAL_GPIO_PIN_LED, (led_pattern & (1U<<led_idx))?1:0);
#elif defined(HAL_GPIO_PIN_SAFE_LED)
// or use safety LED if defined
palWriteLine(HAL_GPIO_PIN_SAFE_LED, (led_pattern & (1U<<led_idx))?1:0);
#else
(void)led_pattern;
(void)led_idx;
#endif
led_idx = (led_idx+1) % 32;
last_led_change = now;
}
if (AP_HAL::millis() > dronecan.send_next_node_id_allocation_request_at_ms) {
can_do_dna();
}
static uint32_t last_1Hz_ms;
if (now - last_1Hz_ms >= 1000) {
last_1Hz_ms = now;
process1HzTasks(AP_HAL::micros64());
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (!hal.run_in_maintenance_mode())
#endif
{
#ifdef HAL_PERIPH_ENABLE_MAG
can_mag_update();
#endif
#ifdef HAL_PERIPH_ENABLE_GPS
can_gps_update();
#endif
#if AP_UART_MONITOR_ENABLED
send_serial_monitor_data();
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY
can_battery_update();
#endif
#ifdef HAL_PERIPH_ENABLE_BARO
can_baro_update();
#endif
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
can_airspeed_update();
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
can_rangefinder_update();
#endif
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
can_proximity_update();
#endif
#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY)
can_buzzer_update();
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
can_safety_LED_update();
#endif
#ifdef HAL_GPIO_PIN_SAFE_BUTTON
can_safety_button_update();
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
pwm_hardpoint_update();
#endif
#ifdef HAL_PERIPH_ENABLE_HWESC
hwesc_telem_update();
#endif
#ifdef HAL_PERIPH_ENABLE_ESC_APD
apd_esc_telem_update();
#endif
#ifdef HAL_PERIPH_ENABLE_MSP
msp_sensor_update();
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
rcout_update();
#endif
#ifdef HAL_PERIPH_ENABLE_EFI
can_efi_update();
#endif
}
const uint32_t now_us = AP_HAL::micros();
while ((AP_HAL::micros() - now_us) < 1000) {
hal.scheduler->delay_microseconds(HAL_PERIPH_LOOP_DELAY_US);
#if HAL_CANFD_SUPPORTED
// allow for user enabling/disabling CANFD at runtime
dronecan.canard.tao_disabled = g.can_fdmode == 1;
#endif
processTx();
processRx();
#if HAL_PERIPH_CAN_MIRROR
processMirror();
#endif // HAL_PERIPH_CAN_MIRROR
}
}
// printf to CAN LogMessage for debugging
void can_printf(const char *fmt, ...)
{
#if HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF
const uint8_t packet_count_max = 4; // how many packets we're willing to break up an over-sized string into
const uint8_t packet_data_max = 90; // max single debug string length = sizeof(uavcan_protocol_debug_LogMessage.text.data)
uint8_t buffer_data[packet_count_max*packet_data_max] {};
va_list ap;
va_start(ap, fmt);
// strip off any negative return errors by treating result as 0
uint32_t char_count = MAX(vsnprintf((char*)buffer_data, sizeof(buffer_data), fmt, ap), 0);
va_end(ap);
// send multiple uavcan_protocol_debug_LogMessage packets if the fmt string is too long.
uint16_t buffer_offset = 0;
for (uint8_t i=0; i<packet_count_max && char_count > 0; i++) {
uavcan_protocol_debug_LogMessage pkt {};
pkt.text.len = MIN(char_count, sizeof(pkt.text.data));
char_count -= pkt.text.len;
memcpy(pkt.text.data, &buffer_data[buffer_offset], pkt.text.len);
buffer_offset += pkt.text.len;
uint8_t buffer_packet[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
const uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer_packet, !periph.canfdout());
periph.canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
CANARD_TRANSFER_PRIORITY_LOW,
buffer_packet,
len);
}
#else
uavcan_protocol_debug_LogMessage pkt {};
uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
va_list ap;
va_start(ap, fmt);
uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
va_end(ap);
pkt.text.len = MIN(n, sizeof(pkt.text.data));
uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, !periph.canfdout());
periph.canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
#endif
}
Reference in New Issue View Git Blame Copy Permalink