mirror of https://github.com/ArduPilot/ardupilot
AP_Periph: added peripheral firmware
This commit is contained in:
Andrew Tridgell
parent
93e27cfcab
commit
d57ba14020
|
|
@ -0,0 +1,98 @@
|
|||
/*
|
||||
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 main firmware
|
||||
|
||||
To flash this firmware on Linux use:
|
||||
|
||||
st-flash write build/f103-periph/bin/AP_Periph.bin 0x8006000
|
||||
|
||||
*/
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_Periph.h"
|
||||
#include "hal.h"
|
||||
#include <stdio.h>
|
||||
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_Periph_FW periph;
|
||||
|
||||
void setup();
|
||||
void loop();
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
periph.init();
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
periph.update();
|
||||
}
|
||||
|
||||
void AP_Periph_FW::init()
|
||||
{
|
||||
hal.uartA->begin(AP_SERIALMANAGER_CONSOLE_BAUD, 32, 128);
|
||||
hal.uartB->begin(115200, 32, 128);
|
||||
|
||||
load_parameters();
|
||||
can_start();
|
||||
|
||||
serial_manager.init();
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
gps.init(serial_manager);
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
compass.init();
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
baro.init();
|
||||
baro.calibrate(false);
|
||||
#endif
|
||||
}
|
||||
|
||||
void AP_Periph_FW::update()
|
||||
{
|
||||
static uint32_t last_led_ms;
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_led_ms > 1000) {
|
||||
last_led_ms = now;
|
||||
palToggleLine(HAL_GPIO_PIN_LED);
|
||||
#if 0
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
hal.uartA->printf("GPS status: %u\n", (unsigned)gps.status());
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
const Vector3f &field = compass.get_field();
|
||||
hal.uartA->printf("MAG (%d,%d,%d)\n", int(field.x), int(field.y), int(field.z));
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
hal.uartA->printf("BARO H=%u P=%.2f T=%.2f\n", baro.healthy(), baro.get_pressure(), baro.get_temperature());
|
||||
#endif
|
||||
hal.scheduler->delay(1);
|
||||
show_stack_usage();
|
||||
#endif
|
||||
}
|
||||
can_update();
|
||||
hal.scheduler->delay(1);
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include "Parameters.h"
|
||||
#include "ch.h"
|
||||
|
||||
class AP_Periph_FW {
|
||||
public:
|
||||
void init();
|
||||
void update();
|
||||
|
||||
Parameters g;
|
||||
|
||||
void can_start();
|
||||
void can_update();
|
||||
void can_mag_update();
|
||||
void can_gps_update();
|
||||
void can_baro_update();
|
||||
|
||||
void load_parameters();
|
||||
|
||||
AP_SerialManager serial_manager;
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
AP_GPS gps;
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
Compass compass;
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
AP_Baro baro;
|
||||
#endif
|
||||
|
||||
// setup the var_info table
|
||||
AP_Param param_loader{var_info};
|
||||
|
||||
static const AP_Param::Info var_info[];
|
||||
|
||||
uint32_t last_mag_update_ms;
|
||||
uint32_t last_gps_update_ms;
|
||||
uint32_t last_baro_update_ms;
|
||||
};
|
||||
|
||||
extern AP_Periph_FW periph;
|
||||
|
||||
extern "C" {
|
||||
void can_printf(const char *fmt, ...);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
/*
|
||||
* AP_Periph parameter definitions
|
||||
*
|
||||
*/
|
||||
|
||||
#define GSCALAR(v, name, def) { periph.g.v.vtype, name, Parameters::k_param_ ## v, &periph.g.v, {def_value : def} }
|
||||
#define ASCALAR(v, name, def) { periph.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&periph.aparm.v, {def_value : def} }
|
||||
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &periph.g.v, {group_info : class::var_info} }
|
||||
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&periph.v, {group_info : class::var_info} }
|
||||
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&periph.v, {group_info : class::var_info} }
|
||||
|
||||
const AP_Param::Info AP_Periph_FW::var_info[] = {
|
||||
// @Param: FORMAT_VERSION
|
||||
// @DisplayName: Eeprom format version number
|
||||
// @Description: This value is incremented when changes are made to the eeprom format
|
||||
// @User: Advanced
|
||||
GSCALAR(format_version, "FORMAT_VERSION", 0),
|
||||
|
||||
// can node number, 0 for dynamic node allocation
|
||||
GSCALAR(can_node, "CAN_NODE", HAL_CAN_DEFAULT_NODE_ID),
|
||||
|
||||
// can node baudrate
|
||||
GSCALAR(can_baudrate, "CAN_BAUDRATE", 1000000),
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
// GPS driver
|
||||
// @Group: GPS_
|
||||
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
|
||||
GOBJECT(gps, "GPS_", AP_GPS),
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
// @Group: COMPASS_
|
||||
// @Path: ../libraries/AP_Compass/AP_Compass.cpp
|
||||
GOBJECT(compass, "COMPASS_", Compass),
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
// Baro driver
|
||||
// @Group: BARO_
|
||||
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
|
||||
GOBJECT(baro, "BARO_", AP_Baro),
|
||||
#endif
|
||||
|
||||
AP_VAREND
|
||||
};
|
||||
|
||||
|
||||
void AP_Periph_FW::load_parameters(void)
|
||||
{
|
||||
AP_Param::setup_sketch_defaults();
|
||||
|
||||
if (!AP_Param::check_var_info()) {
|
||||
hal.console->printf("Bad parameter table\n");
|
||||
AP_HAL::panic("Bad parameter table");
|
||||
}
|
||||
if (!g.format_version.load() ||
|
||||
g.format_version != Parameters::k_format_version) {
|
||||
// erase all parameters
|
||||
StorageManager::erase();
|
||||
AP_Param::erase_all();
|
||||
|
||||
// save the current format version
|
||||
g.format_version.set_and_save(Parameters::k_format_version);
|
||||
}
|
||||
|
||||
// Load all auto-loaded EEPROM variables
|
||||
AP_Param::load_all();
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
|
||||
// Global parameter class.
|
||||
//
|
||||
class Parameters {
|
||||
public:
|
||||
static const uint16_t k_format_version = 2;
|
||||
|
||||
enum {
|
||||
// Layout version number, always key zero.
|
||||
//
|
||||
k_param_format_version = 0,
|
||||
k_param_gps,
|
||||
k_param_compass,
|
||||
k_param_can_node,
|
||||
k_param_can_baudrate,
|
||||
k_param_baro,
|
||||
};
|
||||
|
||||
AP_Int16 format_version;
|
||||
AP_Int16 can_node;
|
||||
AP_Int32 can_baudrate;
|
||||
|
||||
Parameters() {}
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
@ -0,0 +1,921 @@
|
|||
/*
|
||||
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_Periph.h"
|
||||
#include "hal.h"
|
||||
#include <canard.h>
|
||||
#include <uavcan/protocol/dynamic_node_id/Allocation.h>
|
||||
#include <uavcan/protocol/NodeStatus.h>
|
||||
#include <uavcan/protocol/RestartNode.h>
|
||||
#include <uavcan/protocol/GetNodeInfo.h>
|
||||
#include <uavcan/protocol/file/BeginFirmwareUpdate.h>
|
||||
#include <uavcan/protocol/param/GetSet.h>
|
||||
#include <uavcan/protocol/param/ExecuteOpcode.h>
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength.h>
|
||||
#include <uavcan/equipment/gnss/Fix.h>
|
||||
#include <uavcan/equipment/gnss/Auxiliary.h>
|
||||
#include <uavcan/equipment/air_data/StaticPressure.h>
|
||||
#include <uavcan/equipment/air_data/StaticTemperature.h>
|
||||
#include <uavcan/protocol/debug/LogMessage.h>
|
||||
#include <stdio.h>
|
||||
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
|
||||
#include <drivers/stm32/canard_stm32.h>
|
||||
|
||||
#include "i2c.h"
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
extern AP_Periph_FW periph;
|
||||
|
||||
static CanardInstance canard;
|
||||
static uint32_t canard_memory_pool[1024/4];
|
||||
#ifndef HAL_CAN_DEFAULT_NODE_ID
|
||||
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID
|
||||
#endif
|
||||
static uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID;
|
||||
static uint8_t transfer_id;
|
||||
|
||||
#ifndef CAN_APP_VERSION_MAJOR
|
||||
#define CAN_APP_VERSION_MAJOR 1
|
||||
#endif
|
||||
#ifndef CAN_APP_VERSION_MINOR
|
||||
#define CAN_APP_VERSION_MINOR 0
|
||||
#endif
|
||||
#ifndef CAN_APP_NODE_NAME
|
||||
#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Variables used for dynamic node ID allocation.
|
||||
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
|
||||
*/
|
||||
static uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent
|
||||
static uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request
|
||||
|
||||
/*
|
||||
* Node status variables
|
||||
*/
|
||||
static uavcan_protocol_NodeStatus node_status;
|
||||
|
||||
|
||||
/**
|
||||
* Returns a pseudo random float in the range [0, 1].
|
||||
*/
|
||||
static float getRandomFloat(void)
|
||||
{
|
||||
return float(get_random16()) / 0xFFFF;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
get cpu unique ID
|
||||
*/
|
||||
static void readUniqueID(uint8_t* out_uid)
|
||||
{
|
||||
uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH;
|
||||
memset(out_uid, 0, len);
|
||||
hal.util->get_system_id_unformatted(out_uid, len);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle a GET_NODE_INFO request
|
||||
*/
|
||||
static void handle_get_node_info(CanardInstance* ins,
|
||||
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 = CAN_APP_VERSION_MAJOR;
|
||||
pkt.software_version.minor = CAN_APP_VERSION_MINOR;
|
||||
|
||||
readUniqueID(pkt.hardware_version.unique_id);
|
||||
|
||||
char name[strlen(CAN_APP_NODE_NAME)+1];
|
||||
strcpy(name, CAN_APP_NODE_NAME);
|
||||
pkt.name.len = strlen(CAN_APP_NODE_NAME);
|
||||
pkt.name.data = (uint8_t *)name;
|
||||
|
||||
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
|
||||
|
||||
const int16_t resp_res = canardRequestOrRespond(ins,
|
||||
transfer->source_node_id,
|
||||
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_GETNODEINFO_ID,
|
||||
&transfer->transfer_id,
|
||||
transfer->priority,
|
||||
CanardResponse,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
if (resp_res <= 0) {
|
||||
printf("Could not respond to GetNodeInfo: %d\n", resp_res);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
handle parameter GetSet request
|
||||
*/
|
||||
static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer)
|
||||
{
|
||||
uavcan_protocol_param_GetSetRequest req;
|
||||
uint8_t arraybuf[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_NAME_MAX_LENGTH];
|
||||
uint8_t *arraybuf_ptr = arraybuf;
|
||||
if (uavcan_protocol_param_GetSetRequest_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
uavcan_protocol_param_GetSetResponse pkt {};
|
||||
|
||||
uint8_t name[AP_MAX_NAME_SIZE+1] {};
|
||||
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) {
|
||||
strncpy((char *)name, (char *)req.name.data, req.name.len);
|
||||
vp = AP_Param::find((char *)name, &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 *)name, 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 = strlen((char *)name);
|
||||
pkt.name.data = name;
|
||||
}
|
||||
|
||||
uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer);
|
||||
|
||||
canardRequestOrRespond(ins,
|
||||
transfer->source_node_id,
|
||||
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_PARAM_GETSET_ID,
|
||||
&transfer->transfer_id,
|
||||
transfer->priority,
|
||||
CanardResponse,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
handle parameter executeopcode request
|
||||
*/
|
||||
static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* transfer)
|
||||
{
|
||||
uavcan_protocol_param_ExecuteOpcodeRequest req;
|
||||
if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, transfer->payload_len, &req, nullptr) < 0) {
|
||||
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(&periph.gps, periph.gps.var_info);
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
AP_Param::setup_object_defaults(&periph.compass, periph.compass.var_info);
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
AP_Param::setup_object_defaults(&periph.baro, periph.baro.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);
|
||||
|
||||
canardRequestOrRespond(ins,
|
||||
transfer->source_node_id,
|
||||
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID,
|
||||
&transfer->transfer_id,
|
||||
transfer->priority,
|
||||
CanardResponse,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer)
|
||||
{
|
||||
// instant reboot, with backup register used to give bootloader
|
||||
// the node_id we rely on the caller re-sending the firmware
|
||||
// update request to the bootloader
|
||||
set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(ins)));
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
|
||||
static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
|
||||
{
|
||||
// Rule C - updating the randomized time interval
|
||||
send_next_node_id_allocation_request_at_ms =
|
||||
AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
|
||||
(uint32_t)(getRandomFloat() * 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");
|
||||
node_id_allocation_unique_id_offset = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// Copying the unique ID from the message
|
||||
static const uint8_t UniqueIDBitOffset = 8;
|
||||
uint8_t received_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH];
|
||||
uint8_t received_unique_id_len = 0;
|
||||
for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) {
|
||||
assert(received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH);
|
||||
const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U);
|
||||
(void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]);
|
||||
}
|
||||
|
||||
// Obtaining the local unique ID
|
||||
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH];
|
||||
readUniqueID(my_unique_id);
|
||||
|
||||
// Matching the received UID against the local one
|
||||
if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) {
|
||||
printf("Mismatching allocation response\n");
|
||||
node_id_allocation_unique_id_offset = 0;
|
||||
return; // No match, return
|
||||
}
|
||||
|
||||
if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) {
|
||||
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
|
||||
node_id_allocation_unique_id_offset = received_unique_id_len;
|
||||
send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
|
||||
|
||||
printf("Matching allocation response: %d\n", received_unique_id_len);
|
||||
} else {
|
||||
// Allocation complete - copying the allocated node ID from the message
|
||||
uint8_t allocated_node_id = 0;
|
||||
(void) canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id);
|
||||
assert(allocated_node_id <= 127);
|
||||
|
||||
canardSetLocalNodeID(ins, allocated_node_id);
|
||||
printf("Node ID allocated: %d\n", allocated_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This callback is invoked by the library when a new message or request or response is received.
|
||||
*/
|
||||
static void onTransferReceived(CanardInstance* ins,
|
||||
CanardRxTransfer* transfer)
|
||||
{
|
||||
/*
|
||||
* Dynamic node ID allocation protocol.
|
||||
* Taking this branch only if we don't have a node ID, ignoring otherwise.
|
||||
*/
|
||||
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) {
|
||||
if (transfer->transfer_type == CanardTransferTypeBroadcast &&
|
||||
transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
|
||||
handle_allocation_response(ins, transfer);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
switch (transfer->data_type_id) {
|
||||
case UAVCAN_PROTOCOL_GETNODEINFO_ID:
|
||||
handle_get_node_info(ins, transfer);
|
||||
break;
|
||||
|
||||
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
|
||||
handle_begin_firmware_update(ins, transfer);
|
||||
break;
|
||||
|
||||
case UAVCAN_PROTOCOL_RESTARTNODE_ID:
|
||||
printf("RestartNode\n");
|
||||
hal.scheduler->delay(10);
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
|
||||
case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
|
||||
handle_param_getset(ins, transfer);
|
||||
break;
|
||||
|
||||
case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID:
|
||||
handle_param_executeopcode(ins, transfer);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
static bool shouldAcceptTransfer(const CanardInstance* ins,
|
||||
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(ins) == 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;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void processTx(void)
|
||||
{
|
||||
static uint8_t fail_count;
|
||||
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
|
||||
CANTxFrame txmsg {};
|
||||
txmsg.DLC = txf->data_len;
|
||||
memcpy(txmsg.data8, txf->data, 8);
|
||||
txmsg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
|
||||
txmsg.IDE = 1;
|
||||
txmsg.RTR = 0;
|
||||
if (canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE) == MSG_OK) {
|
||||
canardPopTxQueue(&canard);
|
||||
fail_count = 0;
|
||||
} else {
|
||||
// just exit and try again later. If we fail 8 times in a row
|
||||
// then start discarding to prevent the pool filling up
|
||||
if (fail_count < 8) {
|
||||
fail_count++;
|
||||
} else {
|
||||
canardPopTxQueue(&canard);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void processRx(void)
|
||||
{
|
||||
CANRxFrame rxmsg {};
|
||||
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
|
||||
CanardCANFrame rx_frame {};
|
||||
|
||||
//palToggleLine(HAL_GPIO_PIN_LED);
|
||||
|
||||
const uint64_t timestamp = AP_HAL::micros64();
|
||||
memcpy(rx_frame.data, rxmsg.data8, 8);
|
||||
rx_frame.data_len = rxmsg.DLC;
|
||||
if(rxmsg.IDE) {
|
||||
rx_frame.id = CANARD_CAN_FRAME_EFF | rxmsg.EID;
|
||||
} else {
|
||||
rx_frame.id = rxmsg.SID;
|
||||
}
|
||||
canardHandleRxFrame(&canard, &rx_frame, timestamp);
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t pool_peak_percent(void)
|
||||
{
|
||||
const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard);
|
||||
const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks);
|
||||
return peak_percent;
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called at 1 Hz rate from the main loop.
|
||||
*/
|
||||
static void process1HzTasks(uint64_t timestamp_usec)
|
||||
{
|
||||
/*
|
||||
* Purging transfers that are no longer transmitted. This will occasionally free up some memory.
|
||||
*/
|
||||
canardCleanupStaleTransfers(&canard, 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.
|
||||
*/
|
||||
{
|
||||
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
|
||||
node_status.uptime_sec = AP_HAL::millis() / 1000U;
|
||||
|
||||
node_status.vendor_specific_status_code = hal.util->available_memory();
|
||||
|
||||
uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
|
||||
|
||||
const int16_t bc_res = canardBroadcast(&canard,
|
||||
UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_NODESTATUS_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
buffer,
|
||||
len);
|
||||
if (bc_res <= 0) {
|
||||
printf("broadcast fail %d\n", bc_res);
|
||||
} else {
|
||||
//printf("broadcast node status OK\n");
|
||||
}
|
||||
}
|
||||
|
||||
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
|
||||
}
|
||||
|
||||
/*
|
||||
wait for dynamic allocation of node ID
|
||||
*/
|
||||
static void can_wait_node_id(void)
|
||||
{
|
||||
uint8_t node_id_allocation_transfer_id = 0;
|
||||
|
||||
while (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID)
|
||||
{
|
||||
printf("Waiting for dynamic node ID allocation... (pool %u)\n", pool_peak_percent());
|
||||
|
||||
send_next_node_id_allocation_request_at_ms =
|
||||
AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
|
||||
(uint32_t)(getRandomFloat() * UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
|
||||
|
||||
while ((AP_HAL::millis() < send_next_node_id_allocation_request_at_ms) &&
|
||||
(canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID))
|
||||
{
|
||||
processTx();
|
||||
processRx();
|
||||
canardCleanupStaleTransfers(&canard, AP_HAL::micros64());
|
||||
}
|
||||
|
||||
if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// 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 (node_id_allocation_unique_id_offset == 0)
|
||||
{
|
||||
allocation_request[0] |= 1; // First part of unique ID
|
||||
}
|
||||
|
||||
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH];
|
||||
readUniqueID(my_unique_id);
|
||||
|
||||
static const uint8_t MaxLenOfUniqueIDInRequest = 6;
|
||||
uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - node_id_allocation_unique_id_offset);
|
||||
if (uid_size > MaxLenOfUniqueIDInRequest)
|
||||
{
|
||||
uid_size = MaxLenOfUniqueIDInRequest;
|
||||
}
|
||||
|
||||
// Paranoia time
|
||||
assert(node_id_allocation_unique_id_offset < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH);
|
||||
assert(uid_size <= MaxLenOfUniqueIDInRequest);
|
||||
assert(uid_size > 0);
|
||||
assert((uid_size + node_id_allocation_unique_id_offset) <= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH);
|
||||
|
||||
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
|
||||
|
||||
// Broadcasting the request
|
||||
const int16_t bcast_res = canardBroadcast(&canard,
|
||||
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
|
||||
&node_id_allocation_transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&allocation_request[0],
|
||||
(uint16_t) (uid_size + 1));
|
||||
if (bcast_res < 0)
|
||||
{
|
||||
printf("Could not broadcast ID allocation req; error %d\n", bcast_res);
|
||||
}
|
||||
|
||||
// Preparing for timeout; if response is received, this value will be updated from the callback.
|
||||
node_id_allocation_unique_id_offset = 0;
|
||||
}
|
||||
|
||||
printf("Dynamic node ID allocation complete [%d]\n", canardGetLocalNodeID(&canard));
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
static CANConfig cancfg = {
|
||||
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
|
||||
0
|
||||
};
|
||||
|
||||
// calculate optimal CAN timings given PCLK1 and baudrate
|
||||
CanardSTM32CANTimings timings {};
|
||||
canardSTM32ComputeCANTimings(STM32_PCLK1, unsigned(g.can_baudrate), &timings);
|
||||
cancfg.btr = CAN_BTR_SJW(0) |
|
||||
CAN_BTR_TS2(timings.bit_segment_2-1) |
|
||||
CAN_BTR_TS1(timings.bit_segment_1-1) |
|
||||
CAN_BTR_BRP(timings.bit_rate_prescaler-1);
|
||||
|
||||
if (g.can_node >= 0 && g.can_node < 128) {
|
||||
PreferredNodeID = g.can_node;
|
||||
}
|
||||
|
||||
canStart(&CAND1, &cancfg);
|
||||
|
||||
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
|
||||
onTransferReceived, shouldAcceptTransfer, NULL);
|
||||
|
||||
if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) {
|
||||
canardSetLocalNodeID(&canard, PreferredNodeID);
|
||||
}
|
||||
|
||||
// wait for dynamic node ID allocation
|
||||
can_wait_node_id();
|
||||
}
|
||||
|
||||
|
||||
void AP_Periph_FW::can_update()
|
||||
{
|
||||
static uint32_t last_1Hz_ms;
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_1Hz_ms >= 1000) {
|
||||
last_1Hz_ms = now;
|
||||
process1HzTasks(AP_HAL::micros64());
|
||||
}
|
||||
can_mag_update();
|
||||
can_gps_update();
|
||||
can_baro_update();
|
||||
processTx();
|
||||
processRx();
|
||||
}
|
||||
|
||||
/*
|
||||
fix value of a float for canard float16 format
|
||||
*/
|
||||
static void fix_float16(float &f)
|
||||
{
|
||||
*(uint16_t *)&f = canardConvertNativeFloatToFloat16(f);
|
||||
}
|
||||
|
||||
/*
|
||||
update CAN magnetometer
|
||||
*/
|
||||
void AP_Periph_FW::can_mag_update(void)
|
||||
{
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
compass.read();
|
||||
#if 1
|
||||
if (compass.get_count() == 0) {
|
||||
static uint32_t last_probe_ms;
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_probe_ms >= 1000) {
|
||||
last_probe_ms = now;
|
||||
compass.init();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (last_mag_update_ms == compass.last_update_ms()) {
|
||||
return;
|
||||
}
|
||||
static uint8_t counter;
|
||||
if (counter++ != 100) {
|
||||
return;
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
last_mag_update_ms = compass.last_update_ms();
|
||||
const Vector3f &field = compass.get_field();
|
||||
uavcan_equipment_ahrs_MagneticFieldStrength pkt {};
|
||||
|
||||
// the canard dsdl compiler doesn't understand float16
|
||||
for (uint8_t i=0; i<3; i++) {
|
||||
pkt.magnetic_field_ga[i] = field[i] * 0.001;
|
||||
fix_float16(pkt.magnetic_field_ga[i]);
|
||||
}
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer);
|
||||
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
#endif // HAL_PERIPH_ENABLE_MAG
|
||||
}
|
||||
|
||||
/*
|
||||
update CAN GPS
|
||||
*/
|
||||
void AP_Periph_FW::can_gps_update(void)
|
||||
{
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
gps.update();
|
||||
if (last_gps_update_ms == gps.last_message_time_ms()) {
|
||||
return;
|
||||
}
|
||||
last_gps_update_ms = gps.last_message_time_ms();
|
||||
|
||||
/*
|
||||
send Fix packet
|
||||
*/
|
||||
uavcan_equipment_gnss_Fix pkt {};
|
||||
const Location &loc = gps.location();
|
||||
const Vector3f &vel = gps.velocity();
|
||||
|
||||
pkt.timestamp.usec = AP_HAL::micros64();
|
||||
pkt.gnss_timestamp.usec = gps.time_epoch_usec();
|
||||
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC;
|
||||
pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
|
||||
pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
|
||||
pkt.height_ellipsoid_mm = loc.alt * 10;
|
||||
pkt.height_msl_mm = loc.alt * 10;
|
||||
for (uint8_t i=0; i<3; i++) {
|
||||
// the canard dsdl compiler doesn't understand float16
|
||||
pkt.ned_velocity[i] = vel[i];
|
||||
fix_float16(pkt.ned_velocity[i]);
|
||||
}
|
||||
pkt.sats_used = gps.num_sats();
|
||||
switch (gps.status()) {
|
||||
case AP_GPS::GPS_Status::NO_GPS:
|
||||
case AP_GPS::GPS_Status::NO_FIX:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_NO_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_2D_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_3D_FIX;
|
||||
break;
|
||||
}
|
||||
|
||||
float pos_cov[9] {};
|
||||
pkt.position_covariance.data = &pos_cov[0];
|
||||
pkt.position_covariance.len = 9;
|
||||
|
||||
float vacc;
|
||||
if (gps.vertical_accuracy(vacc)) {
|
||||
pos_cov[8] = sq(vacc);
|
||||
fix_float16(pos_cov[8]);
|
||||
}
|
||||
|
||||
float hacc;
|
||||
if (gps.horizontal_accuracy(hacc)) {
|
||||
pos_cov[0] = pos_cov[4] = sq(hacc);
|
||||
fix_float16(pos_cov[0]);
|
||||
fix_float16(pos_cov[4]);
|
||||
}
|
||||
|
||||
float vel_cov[9] {};
|
||||
pkt.velocity_covariance.data = &pos_cov[0];
|
||||
pkt.velocity_covariance.len = 9;
|
||||
|
||||
float sacc;
|
||||
if (gps.speed_accuracy(sacc)) {
|
||||
float vc3 = sq(sacc/3.0);
|
||||
vel_cov[0] = vel_cov[4] = vel_cov[8] = vc3;
|
||||
fix_float16(vel_cov[0]);
|
||||
fix_float16(vel_cov[4]);
|
||||
fix_float16(vel_cov[8]);
|
||||
}
|
||||
|
||||
{
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer);
|
||||
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_GNSS_FIX_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
/*
|
||||
send aux packet
|
||||
*/
|
||||
{
|
||||
uavcan_equipment_gnss_Auxiliary aux {};
|
||||
aux.hdop = gps.get_hdop() * 0.01;
|
||||
aux.vdop = gps.get_vdop() * 0.01;
|
||||
fix_float16(aux.hdop);
|
||||
fix_float16(aux.vdop);
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer);
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
#endif // HAL_PERIPH_ENABLE_GPS
|
||||
}
|
||||
|
||||
/*
|
||||
update CAN baro
|
||||
*/
|
||||
void AP_Periph_FW::can_baro_update(void)
|
||||
{
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
baro.update();
|
||||
if (last_baro_update_ms == baro.get_last_update()) {
|
||||
return;
|
||||
}
|
||||
|
||||
last_baro_update_ms = baro.get_last_update();
|
||||
if (!baro.healthy()) {
|
||||
// don't send any data
|
||||
return;
|
||||
}
|
||||
const float press = baro.get_pressure();
|
||||
const float temp = baro.get_temperature();
|
||||
|
||||
{
|
||||
uavcan_equipment_air_data_StaticPressure pkt {};
|
||||
pkt.static_pressure = press;
|
||||
pkt.static_pressure_variance = 0; // should we make this a parameter?
|
||||
fix_float16(pkt.static_pressure_variance);
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer);
|
||||
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
{
|
||||
uavcan_equipment_air_data_StaticTemperature pkt {};
|
||||
pkt.static_temperature = temp + C_TO_KELVIN;
|
||||
pkt.static_temperature_variance = 0; // should we make this a parameter?
|
||||
|
||||
fix_float16(pkt.static_temperature);
|
||||
fix_float16(pkt.static_temperature_variance);
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE];
|
||||
uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer);
|
||||
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
#endif // HAL_PERIPH_ENABLE_BARO
|
||||
}
|
||||
|
||||
// printf to CAN LogMessage for debugging
|
||||
void can_printf(const char *fmt, ...)
|
||||
{
|
||||
uavcan_protocol_debug_LogMessage pkt {};
|
||||
uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE];
|
||||
char tbuf[100];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
uint32_t n = vsnprintf(tbuf, sizeof(tbuf), fmt, ap);
|
||||
va_end(ap);
|
||||
pkt.text.len = n;
|
||||
pkt.text.data = (uint8_t *)&tbuf[0];
|
||||
|
||||
uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer);
|
||||
|
||||
canardBroadcast(&canard,
|
||||
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
|
||||
&transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
buffer,
|
||||
len);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
bool i2c_transfer(uint8_t address,
|
||||
const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len);
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
if not bld.env.BOARD in ['f103-periph', 'CUAV_GPS']:
|
||||
return
|
||||
|
||||
bld.ap_stlib(
|
||||
name= 'AP_Periph_libs',
|
||||
ap_vehicle='AP_Periph',
|
||||
ap_libraries= [
|
||||
'AP_Buffer',
|
||||
'AP_Common',
|
||||
'AP_HAL',
|
||||
'AP_HAL_Empty',
|
||||
'AP_Math',
|
||||
'AP_BoardConfig',
|
||||
'AP_Param',
|
||||
'StorageManager',
|
||||
'AP_FlashStorage',
|
||||
'AP_GPS',
|
||||
'AP_SerialManager',
|
||||
'AP_RTC',
|
||||
'AP_Compass',
|
||||
'AP_Baro',
|
||||
'Filter',
|
||||
'AP_InternalError',
|
||||
],
|
||||
exclude_src=[
|
||||
'libraries/AP_HAL_ChibiOS/Storage.cpp'
|
||||
]
|
||||
)
|
||||
|
||||
# build external libcanard library
|
||||
bld.stlib(source='../../modules/libcanard/canard.c',
|
||||
target='libcanard')
|
||||
|
||||
bld.ap_program(
|
||||
program_name='AP_Periph',
|
||||
use=['AP_Periph_libs', 'libcanard'],
|
||||
program_groups=['bin','AP_Periph'],
|
||||
includes=[bld.env.SRCROOT + '/modules/libcanard',
|
||||
bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated']
|
||||
)
|
||||
|
||||
bld(
|
||||
# build libcanard headers
|
||||
source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"),
|
||||
rule="python3 ../../modules/libcanard/dsdl_compiler/libcanard_dsdlc --header_only --outdir ${BUILDROOT}/modules/libcanard/dsdlc_generated ../../modules/uavcan/dsdl/uavcan",
|
||||
group='dynamic_sources',
|
||||
)
|
||||
Loading…
Reference in New Issue