mirror of https://github.com/ArduPilot/ardupilot
AP_Periph: re-structure peripheral code
split into separate cpp files and avoid static functions
This commit is contained in:
Andrew Tridgell
parent
c0cd255135
commit
0c38dada6c
|
|
@ -231,7 +231,7 @@ void AP_Periph_FW::init()
|
|||
}
|
||||
#endif
|
||||
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
if (proximity.get_type(0) != AP_Proximity::Type::None && g.proximity_port >= 0) {
|
||||
auto *uart = hal.serial(g.proximity_port);
|
||||
if (uart != nullptr) {
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <canard.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
|
|
@ -85,6 +86,21 @@ void can_printf(const char *fmt, ...) FMT_PRINTF(1,2);
|
|||
struct CanardInstance;
|
||||
struct CanardRxTransfer;
|
||||
|
||||
#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id) \
|
||||
(((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) | \
|
||||
(((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U))
|
||||
|
||||
#ifndef HAL_CAN_POOL_SIZE
|
||||
#if HAL_CANFD_SUPPORTED
|
||||
#define HAL_CAN_POOL_SIZE 16000
|
||||
#elif GPS_MOVING_BASELINE
|
||||
#define HAL_CAN_POOL_SIZE 8000
|
||||
#else
|
||||
#define HAL_CAN_POOL_SIZE 4000
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
class AP_Periph_FW {
|
||||
public:
|
||||
AP_Periph_FW();
|
||||
|
|
@ -115,6 +131,9 @@ public:
|
|||
void can_rangefinder_update();
|
||||
void can_battery_update();
|
||||
void can_proximity_update();
|
||||
void can_buzzer_update(void);
|
||||
void can_safety_button_update(void);
|
||||
void can_safety_LED_update(void);
|
||||
|
||||
void load_parameters();
|
||||
void prepare_reboot();
|
||||
|
|
@ -221,7 +240,7 @@ public:
|
|||
uint32_t last_sample_ms;
|
||||
#endif
|
||||
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
AP_Proximity proximity;
|
||||
#endif
|
||||
|
||||
|
|
@ -366,12 +385,20 @@ public:
|
|||
static bool no_iface_finished_dna;
|
||||
static constexpr auto can_printf = ::can_printf;
|
||||
|
||||
static bool canard_broadcast(uint64_t data_type_signature,
|
||||
bool canard_broadcast(uint64_t data_type_signature,
|
||||
uint16_t data_type_id,
|
||||
uint8_t priority,
|
||||
const void* payload,
|
||||
uint16_t payload_len);
|
||||
|
||||
void onTransferReceived(CanardInstance* canard_instance,
|
||||
CanardRxTransfer* transfer);
|
||||
bool shouldAcceptTransfer(const CanardInstance* canard_instance,
|
||||
uint64_t* out_data_type_signature,
|
||||
uint16_t data_type_id,
|
||||
CanardTransferType transfer_type,
|
||||
uint8_t source_node_id);
|
||||
|
||||
#if AP_UART_MONITOR_ENABLED
|
||||
void handle_tunnel_Targetted(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void send_serial_monitor_data();
|
||||
|
|
@ -389,6 +416,51 @@ public:
|
|||
} uart_monitor;
|
||||
#endif
|
||||
|
||||
// handlers for incoming messages
|
||||
void handle_get_node_info(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_param_getset(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_param_executeopcode(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_allocation_response(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_safety_state(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_arming_status(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_esc_rawcommand(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_act_command(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
void handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer);
|
||||
|
||||
void process1HzTasks(uint64_t timestamp_usec);
|
||||
void processTx(void);
|
||||
void processRx(void);
|
||||
void cleanup_stale_transactions(uint64_t ×tamp_usec);
|
||||
void update_rx_protocol_stats(int16_t res);
|
||||
void node_status_send(void);
|
||||
bool can_do_dna();
|
||||
uint8_t *get_tid_ptr(uint32_t transfer_desc);
|
||||
uint16_t pool_peak_percent();
|
||||
void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue);
|
||||
|
||||
struct dronecan_protocol_t {
|
||||
CanardInstance canard;
|
||||
uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)];
|
||||
struct tid_map {
|
||||
uint32_t transfer_desc;
|
||||
uint8_t tid;
|
||||
tid_map *next;
|
||||
} *tid_map_head;
|
||||
/*
|
||||
* Variables used for dynamic node ID allocation.
|
||||
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
|
||||
*/
|
||||
uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent
|
||||
uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request
|
||||
uint8_t tx_fail_count;
|
||||
uint8_t dna_interface = 1;
|
||||
} dronecan;
|
||||
|
||||
#if AP_SIM_ENABLED
|
||||
SITL::SIM sitl;
|
||||
#if AP_AHRS_ENABLED
|
||||
|
|
|
|||
|
|
@ -475,7 +475,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
|
|||
GOBJECT(efi, "EFI", AP_EFI),
|
||||
#endif
|
||||
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
// @Param: PRX_BAUDRATE
|
||||
// @DisplayName: Proximity Sensor serial baudrate
|
||||
// @Description: Proximity Sensor serial baudrate.
|
||||
|
|
@ -507,7 +507,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
|
|||
// @Group: PRX
|
||||
// @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
|
||||
GOBJECT(proximity, "PRX", AP_Proximity),
|
||||
#endif // HAL_PROXIMITY_ENABLED
|
||||
#endif // HAL_PERIPH_ENABLE_PROXIMITY
|
||||
|
||||
#if HAL_NMEA_OUTPUT_ENABLED
|
||||
// @Group: NMEA_
|
||||
|
|
|
|||
|
|
@ -115,7 +115,7 @@ public:
|
|||
AP_Int16 rangefinder_max_rate;
|
||||
#endif
|
||||
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
AP_Int32 proximity_baud;
|
||||
AP_Int8 proximity_port;
|
||||
AP_Int16 proximity_max_rate;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
|
||||
|
||||
/*
|
||||
airspeed support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
#ifndef AP_PERIPH_PROBE_CONTINUOUS
|
||||
#define AP_PERIPH_PROBE_CONTINUOUS 0
|
||||
#endif
|
||||
|
||||
/*
|
||||
update CAN airspeed
|
||||
*/
|
||||
void AP_Periph_FW::can_airspeed_update(void)
|
||||
{
|
||||
if (!airspeed.enabled()) {
|
||||
return;
|
||||
}
|
||||
#if AP_PERIPH_PROBE_CONTINUOUS
|
||||
if (!airspeed.healthy()) {
|
||||
uint32_t now = AP_HAL::millis();
|
||||
static uint32_t last_probe_ms;
|
||||
if (now - last_probe_ms >= 1000) {
|
||||
last_probe_ms = now;
|
||||
airspeed.allocate();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_airspeed_update_ms < 50) {
|
||||
// max 20Hz data
|
||||
return;
|
||||
}
|
||||
last_airspeed_update_ms = now;
|
||||
airspeed.update();
|
||||
if (!airspeed.healthy()) {
|
||||
// don't send any data
|
||||
return;
|
||||
}
|
||||
const float press = airspeed.get_corrected_pressure();
|
||||
float temp;
|
||||
if (!airspeed.get_temperature(temp)) {
|
||||
temp = nanf("");
|
||||
} else {
|
||||
temp = C_TO_KELVIN(temp);
|
||||
}
|
||||
|
||||
uavcan_equipment_air_data_RawAirData pkt {};
|
||||
|
||||
// unfilled elements are NaN
|
||||
pkt.static_pressure = nanf("");
|
||||
pkt.static_pressure_sensor_temperature = nanf("");
|
||||
pkt.differential_pressure_sensor_temperature = nanf("");
|
||||
pkt.pitot_temperature = nanf("");
|
||||
|
||||
// populate the elements we have
|
||||
pkt.differential_pressure = press;
|
||||
pkt.static_air_temperature = temp;
|
||||
|
||||
// if a Pitot tube temperature sensor is available, use it
|
||||
#if AP_TEMPERATURE_SENSOR_ENABLED
|
||||
for (uint8_t i=0; i<temperature_sensor.num_instances(); i++) {
|
||||
float temp_pitot;
|
||||
if (temperature_sensor.get_source(i) == AP_TemperatureSensor_Params::Source::Pitot_tube && temperature_sensor.get_temperature(temp_pitot, i)) {
|
||||
pkt.pitot_temperature = C_TO_KELVIN(temp_pitot);
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_AIRSPEED
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
|
||||
/*
|
||||
barometer support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
/*
|
||||
update CAN baro
|
||||
*/
|
||||
void AP_Periph_FW::can_baro_update(void)
|
||||
{
|
||||
if (!periph.g.baro_enable) {
|
||||
return;
|
||||
}
|
||||
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?
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
{
|
||||
uavcan_equipment_air_data_StaticTemperature pkt {};
|
||||
pkt.static_temperature = C_TO_KELVIN(temp);
|
||||
pkt.static_temperature_variance = 0; // should we make this a parameter?
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_BARO
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BATTERY
|
||||
|
||||
/*
|
||||
battery support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
#ifndef AP_PERIPH_BATTERY_MODEL_NAME
|
||||
#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME
|
||||
#endif
|
||||
|
||||
/*
|
||||
update CAN battery monitor
|
||||
*/
|
||||
void AP_Periph_FW::can_battery_update(void)
|
||||
{
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - battery.last_can_send_ms < 100) {
|
||||
return;
|
||||
}
|
||||
battery.last_can_send_ms = now_ms;
|
||||
|
||||
const uint8_t battery_instances = battery_lib.num_instances();
|
||||
for (uint8_t i=0; i<battery_instances; i++) {
|
||||
if (!battery_lib.healthy(i)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uavcan_equipment_power_BatteryInfo pkt {};
|
||||
|
||||
// if a battery serial number is assigned, use that as the ID. Else, use the index.
|
||||
const int32_t serial_number = battery_lib.get_serial_number(i);
|
||||
pkt.battery_id = (serial_number >= 0) ? serial_number : i+1;
|
||||
|
||||
pkt.voltage = battery_lib.voltage(i);
|
||||
|
||||
float current;
|
||||
if (battery_lib.current_amps(current, i)) {
|
||||
pkt.current = current;
|
||||
}
|
||||
float temperature;
|
||||
if (battery_lib.get_temperature(temperature, i)) {
|
||||
// Battery lib reports temperature in Celsius.
|
||||
// Convert Celsius to Kelvin for transmission on CAN.
|
||||
pkt.temperature = C_TO_KELVIN(temperature);
|
||||
}
|
||||
|
||||
pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN;
|
||||
uint8_t percentage = 0;
|
||||
if (battery_lib.capacity_remaining_pct(percentage, i)) {
|
||||
pkt.state_of_charge_pct = percentage;
|
||||
}
|
||||
pkt.model_instance_id = i+1;
|
||||
|
||||
#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME)
|
||||
// example model_name: "org.ardupilot.ap_periph SN 123"
|
||||
hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number);
|
||||
pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data));
|
||||
#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME)
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {};
|
||||
const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_BATTERY
|
||||
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY)
|
||||
|
||||
/*
|
||||
buzzer support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
static uint32_t buzzer_start_ms;
|
||||
static uint32_t buzzer_len_ms;
|
||||
|
||||
/*
|
||||
handle BeepCommand
|
||||
*/
|
||||
void AP_Periph_FW::handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer)
|
||||
{
|
||||
uavcan_equipment_indication_BeepCommand req;
|
||||
if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) {
|
||||
return;
|
||||
}
|
||||
static bool initialised;
|
||||
if (!initialised) {
|
||||
initialised = true;
|
||||
hal.rcout->init();
|
||||
hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin);
|
||||
}
|
||||
buzzer_start_ms = AP_HAL::millis();
|
||||
buzzer_len_ms = req.duration*1000;
|
||||
#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY
|
||||
float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1);
|
||||
#elif defined(HAL_PERIPH_ENABLE_NOTIFY)
|
||||
float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1);
|
||||
#endif
|
||||
hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000));
|
||||
}
|
||||
|
||||
/*
|
||||
update buzzer
|
||||
*/
|
||||
void AP_Periph_FW::can_buzzer_update(void)
|
||||
{
|
||||
if (buzzer_start_ms != 0) {
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - buzzer_start_ms > buzzer_len_ms) {
|
||||
hal.util->toneAlarm_set_buzzer_tone(0, 0, 0);
|
||||
buzzer_start_ms = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY)
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,75 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
|
||||
/*
|
||||
magnetometer support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
#ifndef AP_PERIPH_MAG_MAX_RATE
|
||||
#define AP_PERIPH_MAG_MAX_RATE 25U
|
||||
#endif
|
||||
|
||||
#ifndef AP_PERIPH_PROBE_CONTINUOUS
|
||||
#define AP_PERIPH_PROBE_CONTINUOUS 0
|
||||
#endif
|
||||
|
||||
/*
|
||||
update CAN magnetometer
|
||||
*/
|
||||
void AP_Periph_FW::can_mag_update(void)
|
||||
{
|
||||
if (!compass.available()) {
|
||||
return;
|
||||
}
|
||||
|
||||
#if AP_PERIPH_MAG_MAX_RATE > 0
|
||||
// don't flood the bus with very high rate magnetometers
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
compass.read();
|
||||
|
||||
#if AP_PERIPH_PROBE_CONTINUOUS
|
||||
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;
|
||||
}
|
||||
if (!compass.healthy()) {
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_MAG
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_EFI
|
||||
|
||||
/*
|
||||
EFI support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
/*
|
||||
update CAN EFI
|
||||
*/
|
||||
void AP_Periph_FW::can_efi_update(void)
|
||||
{
|
||||
if (!efi.enabled()) {
|
||||
return;
|
||||
}
|
||||
efi.update();
|
||||
const uint32_t update_ms = efi.get_last_update_ms();
|
||||
if (!efi.is_healthy() || efi_update_ms == update_ms) {
|
||||
return;
|
||||
}
|
||||
efi_update_ms = update_ms;
|
||||
EFI_State state;
|
||||
efi.get_state(state);
|
||||
|
||||
{
|
||||
/*
|
||||
send status packet
|
||||
*/
|
||||
uavcan_equipment_ice_reciprocating_Status pkt {};
|
||||
|
||||
// state maps 1:1 from Engine_State
|
||||
pkt.state = uint8_t(state.engine_state);
|
||||
|
||||
switch (state.crankshaft_sensor_status) {
|
||||
case Crankshaft_Sensor_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Crankshaft_Sensor_Status::OK:
|
||||
pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED;
|
||||
break;
|
||||
case Crankshaft_Sensor_Status::ERROR:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.temperature_status) {
|
||||
case Temperature_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Temperature_Status::OK:
|
||||
pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED;
|
||||
break;
|
||||
case Temperature_Status::BELOW_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL;
|
||||
break;
|
||||
case Temperature_Status::ABOVE_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL;
|
||||
break;
|
||||
case Temperature_Status::OVERHEATING:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING;
|
||||
break;
|
||||
case Temperature_Status::EGT_ABOVE_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.fuel_pressure_status) {
|
||||
case Fuel_Pressure_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Fuel_Pressure_Status::OK:
|
||||
pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED;
|
||||
break;
|
||||
case Fuel_Pressure_Status::BELOW_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL;
|
||||
break;
|
||||
case Fuel_Pressure_Status::ABOVE_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.oil_pressure_status) {
|
||||
case Oil_Pressure_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Oil_Pressure_Status::OK:
|
||||
pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED;
|
||||
break;
|
||||
case Oil_Pressure_Status::BELOW_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL;
|
||||
break;
|
||||
case Oil_Pressure_Status::ABOVE_NOMINAL:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.detonation_status) {
|
||||
case Detonation_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Detonation_Status::NOT_OBSERVED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED;
|
||||
break;
|
||||
case Detonation_Status::OBSERVED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.misfire_status) {
|
||||
case Misfire_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Misfire_Status::NOT_OBSERVED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED;
|
||||
break;
|
||||
case Misfire_Status::OBSERVED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (state.debris_status) {
|
||||
case Debris_Status::NOT_SUPPORTED:
|
||||
break;
|
||||
case Debris_Status::NOT_DETECTED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED;
|
||||
break;
|
||||
case Debris_Status::DETECTED:
|
||||
pkt.flags |=
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED |
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED;
|
||||
break;
|
||||
}
|
||||
|
||||
pkt.engine_load_percent = state.engine_load_percent;
|
||||
pkt.engine_speed_rpm = state.engine_speed_rpm;
|
||||
pkt.spark_dwell_time_ms = state.spark_dwell_time_ms;
|
||||
pkt.atmospheric_pressure_kpa = state.atmospheric_pressure_kpa;
|
||||
pkt.intake_manifold_pressure_kpa = state.intake_manifold_pressure_kpa;
|
||||
pkt.intake_manifold_temperature = state.intake_manifold_temperature;
|
||||
pkt.coolant_temperature = state.coolant_temperature;
|
||||
pkt.oil_pressure = state.oil_pressure;
|
||||
pkt.oil_temperature = state.oil_temperature;
|
||||
pkt.fuel_pressure = state.fuel_pressure;
|
||||
pkt.fuel_consumption_rate_cm3pm = state.fuel_consumption_rate_cm3pm;
|
||||
pkt.estimated_consumed_fuel_volume_cm3 = state.estimated_consumed_fuel_volume_cm3;
|
||||
pkt.throttle_position_percent = state.throttle_position_percent;
|
||||
pkt.ecu_index = state.ecu_index;
|
||||
pkt.spark_plug_usage = uint8_t(state.spark_plug_usage);
|
||||
|
||||
// assume single set of cylinder status
|
||||
pkt.cylinder_status.len = 1;
|
||||
auto &c = pkt.cylinder_status.data[0];
|
||||
const auto &state_c = state.cylinder_status;
|
||||
c.ignition_timing_deg = state_c.ignition_timing_deg;
|
||||
c.injection_time_ms = state_c.injection_time_ms;
|
||||
c.cylinder_head_temperature = state_c.cylinder_head_temperature;
|
||||
c.exhaust_gas_temperature = state_c.exhaust_gas_temperature;
|
||||
c.lambda_coefficient = state_c.lambda_coefficient;
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_MAX_SIZE] {};
|
||||
const uint16_t total_size = uavcan_equipment_ice_reciprocating_Status_encode(&pkt, buffer, !canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_EFI
|
||||
|
|
@ -0,0 +1,322 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
|
||||
/*
|
||||
GPS support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
#include <AP_GPS/RTCM3_Parser.h>
|
||||
|
||||
#define DEBUG_PRINTS 0
|
||||
|
||||
#if DEBUG_PRINTS
|
||||
# define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0)
|
||||
#else
|
||||
# define Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
/*
|
||||
handle gnss::RTCMStream
|
||||
*/
|
||||
void AP_Periph_FW::handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer)
|
||||
{
|
||||
uavcan_equipment_gnss_RTCMStream req;
|
||||
if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) {
|
||||
return;
|
||||
}
|
||||
gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len);
|
||||
}
|
||||
|
||||
/*
|
||||
handle gnss::MovingBaselineData
|
||||
*/
|
||||
#if GPS_MOVING_BASELINE
|
||||
void AP_Periph_FW::handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer)
|
||||
{
|
||||
ardupilot_gnss_MovingBaselineData msg;
|
||||
if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) {
|
||||
return;
|
||||
}
|
||||
gps.inject_MBL_data(msg.data.data, msg.data.len);
|
||||
Debug("MovingBaselineData: len=%u\n", msg.data.len);
|
||||
}
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
|
||||
/*
|
||||
convert large values to NaN for float16
|
||||
*/
|
||||
static void check_float16_range(float *v, uint8_t len)
|
||||
{
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
const float f16max = 65519;
|
||||
if (isinf(v[i]) || v[i] <= -f16max || v[i] >= f16max) {
|
||||
v[i] = nanf("");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update CAN GPS
|
||||
*/
|
||||
void AP_Periph_FW::can_gps_update(void)
|
||||
{
|
||||
if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
|
||||
return;
|
||||
}
|
||||
gps.update();
|
||||
send_moving_baseline_msg();
|
||||
send_relposheading_msg();
|
||||
if (last_gps_update_ms == gps.last_message_time_ms()) {
|
||||
return;
|
||||
}
|
||||
last_gps_update_ms = gps.last_message_time_ms();
|
||||
|
||||
{
|
||||
/*
|
||||
send Fix2 packet
|
||||
*/
|
||||
uavcan_equipment_gnss_Fix2 pkt {};
|
||||
const Location &loc = gps.location();
|
||||
const Vector3f &vel = gps.velocity();
|
||||
if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) {
|
||||
pkt.timestamp.usec = AP_HAL::micros64();
|
||||
pkt.gnss_timestamp.usec = 0;
|
||||
} else {
|
||||
saw_gps_lock_once = true;
|
||||
pkt.timestamp.usec = gps.time_epoch_usec();
|
||||
pkt.gnss_timestamp.usec = gps.last_message_epoch_usec();
|
||||
}
|
||||
if (pkt.gnss_timestamp.usec == 0) {
|
||||
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE;
|
||||
} else {
|
||||
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;
|
||||
float undulation;
|
||||
if (gps.get_undulation(undulation)) {
|
||||
pkt.height_ellipsoid_mm -= undulation*1000;
|
||||
}
|
||||
for (uint8_t i=0; i<3; i++) {
|
||||
pkt.ned_velocity[i] = vel[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_FIX2_STATUS_NO_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
|
||||
pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
|
||||
pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED;
|
||||
break;
|
||||
}
|
||||
|
||||
pkt.covariance.len = 6;
|
||||
|
||||
float hacc;
|
||||
if (gps.horizontal_accuracy(hacc)) {
|
||||
pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc);
|
||||
}
|
||||
|
||||
float vacc;
|
||||
if (gps.vertical_accuracy(vacc)) {
|
||||
pkt.covariance.data[2] = sq(vacc);
|
||||
}
|
||||
|
||||
float sacc;
|
||||
if (gps.speed_accuracy(sacc)) {
|
||||
float vc3 = sq(sacc);
|
||||
pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;
|
||||
}
|
||||
|
||||
check_float16_range(pkt.covariance.data, pkt.covariance.len);
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_GNSS_FIX2_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;
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !canfdout());
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
// send the gnss status packet
|
||||
{
|
||||
ardupilot_gnss_Status status {};
|
||||
|
||||
status.healthy = gps.is_healthy();
|
||||
if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
|
||||
status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;
|
||||
}
|
||||
uint8_t idx; // unused
|
||||
if (status.healthy && !gps.first_unconfigured_gps(idx)) {
|
||||
status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;
|
||||
}
|
||||
|
||||
uint32_t error_codes;
|
||||
if (gps.get_error_codes(error_codes)) {
|
||||
status.error_codes = error_codes;
|
||||
}
|
||||
|
||||
uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {};
|
||||
const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !canfdout());
|
||||
canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE,
|
||||
ARDUPILOT_GNSS_STATUS_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
|
||||
}
|
||||
|
||||
// send Heading message if we are not sending RelPosHeading messages and have yaw
|
||||
if (gps.have_gps_yaw() && last_relposheading_ms == 0) {
|
||||
float yaw_deg, yaw_acc_deg;
|
||||
uint32_t yaw_time_ms;
|
||||
if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) {
|
||||
last_gps_yaw_ms = yaw_time_ms;
|
||||
|
||||
ardupilot_gnss_Heading heading {};
|
||||
heading.heading_valid = true;
|
||||
heading.heading_accuracy_valid = is_positive(yaw_acc_deg);
|
||||
heading.heading_rad = radians(yaw_deg);
|
||||
heading.heading_accuracy_rad = radians(yaw_acc_deg);
|
||||
uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE] {};
|
||||
const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !canfdout());
|
||||
canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE,
|
||||
ARDUPILOT_GNSS_HEADING_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AP_Periph_FW::send_moving_baseline_msg()
|
||||
{
|
||||
#if GPS_MOVING_BASELINE
|
||||
const uint8_t *data = nullptr;
|
||||
uint16_t len = 0;
|
||||
if (!gps.get_RTCMV3(data, len)) {
|
||||
return;
|
||||
}
|
||||
if (len == 0 || data == nullptr) {
|
||||
return;
|
||||
}
|
||||
// send the packet from Moving Base to be used RelPosHeading calc by GPS module
|
||||
ardupilot_gnss_MovingBaselineData mbldata {};
|
||||
// get the data from the moving base
|
||||
static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong");
|
||||
mbldata.data.len = len;
|
||||
memcpy(mbldata.data.data, data, len);
|
||||
uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
|
||||
const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout());
|
||||
|
||||
#if HAL_NUM_CAN_IFACES >= 2
|
||||
if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) {
|
||||
uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID));
|
||||
canardBroadcast(&dronecan.canard,
|
||||
ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
|
||||
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
|
||||
tid_ptr,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size
|
||||
#if CANARD_MULTI_IFACE
|
||||
,(1U<<gps_mb_can_port)
|
||||
#endif
|
||||
#if HAL_CANFD_SUPPORTED
|
||||
,canfdout()
|
||||
#endif
|
||||
);
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
// we use MEDIUM priority on this data as we need to get all
|
||||
// the data through for RTK moving baseline yaw to work
|
||||
canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
|
||||
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
|
||||
CANARD_TRANSFER_PRIORITY_MEDIUM,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
gps.clear_RTCMV3();
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
}
|
||||
|
||||
void AP_Periph_FW::send_relposheading_msg() {
|
||||
#if GPS_MOVING_BASELINE
|
||||
float reported_heading;
|
||||
float relative_distance;
|
||||
float relative_down_pos;
|
||||
float reported_heading_acc;
|
||||
uint32_t curr_timestamp = 0;
|
||||
gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc);
|
||||
if (last_relposheading_ms == curr_timestamp) {
|
||||
return;
|
||||
}
|
||||
last_relposheading_ms = curr_timestamp;
|
||||
ardupilot_gnss_RelPosHeading relpos {};
|
||||
relpos.timestamp.usec = uint64_t(curr_timestamp)*1000LLU;
|
||||
relpos.reported_heading_deg = reported_heading;
|
||||
relpos.relative_distance_m = relative_distance;
|
||||
relpos.relative_down_pos_m = relative_down_pos;
|
||||
relpos.reported_heading_acc_deg = reported_heading_acc;
|
||||
relpos.reported_heading_acc_available = !is_zero(relpos.reported_heading_acc_deg);
|
||||
uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE] {};
|
||||
const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer, !canfdout());
|
||||
canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE,
|
||||
ARDUPILOT_GNSS_RELPOSHEADING_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_GPS
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
|
||||
|
||||
/*
|
||||
hardpoint support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
void AP_Periph_FW::pwm_hardpoint_init()
|
||||
{
|
||||
hal.gpio->attach_interrupt(
|
||||
PWM_HARDPOINT_PIN,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH);
|
||||
}
|
||||
|
||||
/*
|
||||
called on PWM pin transition
|
||||
*/
|
||||
void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
|
||||
{
|
||||
if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) {
|
||||
uint32_t width = timestamp - pwm_hardpoint.last_ts_us;
|
||||
if (width > 500 && width < 2500) {
|
||||
pwm_hardpoint.pwm_value = width;
|
||||
if (width > pwm_hardpoint.highest_pwm) {
|
||||
pwm_hardpoint.highest_pwm = width;
|
||||
}
|
||||
}
|
||||
}
|
||||
pwm_hardpoint.last_state = pin_state;
|
||||
pwm_hardpoint.last_ts_us = timestamp;
|
||||
}
|
||||
|
||||
void AP_Periph_FW::pwm_hardpoint_update()
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
// send at 10Hz
|
||||
void *save = hal.scheduler->disable_interrupts_save();
|
||||
uint16_t value = pwm_hardpoint.highest_pwm;
|
||||
pwm_hardpoint.highest_pwm = 0;
|
||||
hal.scheduler->restore_interrupts(save);
|
||||
float rate = g.hardpoint_rate;
|
||||
rate = constrain_float(rate, 10, 100);
|
||||
if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) {
|
||||
pwm_hardpoint.last_send_ms = now;
|
||||
uavcan_equipment_hardpoint_Command cmd {};
|
||||
cmd.hardpoint_id = g.hardpoint_id;
|
||||
cmd.command = value;
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !canfdout());
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT
|
||||
|
|
@ -6,8 +6,11 @@
|
|||
This protocol only allows for one ESC per UART RX line, so using a
|
||||
CAN node per ESC works well.
|
||||
*/
|
||||
|
||||
#include "AP_Periph.h"
|
||||
#include "hwing_esc.h"
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_HWESC
|
||||
|
||||
|
|
@ -143,5 +146,30 @@ bool HWESC_Telem::parse_packet(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
void AP_Periph_FW::hwesc_telem_update()
|
||||
{
|
||||
if (!hwesc_telem.update()) {
|
||||
return;
|
||||
}
|
||||
const HWESC_Telem::HWESC &t = hwesc_telem.get_telem();
|
||||
|
||||
uavcan_equipment_esc_Status pkt {};
|
||||
pkt.esc_index = g.esc_number[0]; // only supports a single ESC
|
||||
pkt.voltage = t.voltage;
|
||||
pkt.current = t.current;
|
||||
pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature));
|
||||
pkt.rpm = t.rpm;
|
||||
pkt.power_rating_pct = t.phase_current;
|
||||
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_HWESC
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,75 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
|
||||
/*
|
||||
proximity support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
void AP_Periph_FW::can_proximity_update()
|
||||
{
|
||||
if (proximity.get_type(0) == AP_Proximity::Type::None) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
static uint32_t last_update_ms;
|
||||
if (g.proximity_max_rate > 0 &&
|
||||
now - last_update_ms < 1000/g.proximity_max_rate) {
|
||||
// limit to max rate
|
||||
return;
|
||||
}
|
||||
last_update_ms = now;
|
||||
proximity.update();
|
||||
AP_Proximity::Status status = proximity.get_status();
|
||||
if (status <= AP_Proximity::Status::NoData) {
|
||||
// don't send any data
|
||||
return;
|
||||
}
|
||||
|
||||
ardupilot_equipment_proximity_sensor_Proximity pkt {};
|
||||
|
||||
const uint8_t obstacle_count = proximity.get_obstacle_count();
|
||||
|
||||
// if no objects return
|
||||
if (obstacle_count == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate maximum roll, pitch values from objects
|
||||
for (uint8_t i=0; i<obstacle_count; i++) {
|
||||
if (!proximity.get_obstacle_info(i, pkt.yaw, pkt.pitch, pkt.distance)) {
|
||||
// not a valid obstacle
|
||||
continue;
|
||||
}
|
||||
|
||||
pkt.sensor_id = proximity.get_address(0);
|
||||
|
||||
switch (status) {
|
||||
case AP_Proximity::Status::NotConnected:
|
||||
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED;
|
||||
break;
|
||||
case AP_Proximity::Status::Good:
|
||||
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD;
|
||||
break;
|
||||
case AP_Proximity::Status::NoData:
|
||||
default:
|
||||
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA;
|
||||
break;
|
||||
}
|
||||
|
||||
uint8_t buffer[ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_MAX_SIZE] {};
|
||||
uint16_t total_size = ardupilot_equipment_proximity_sensor_Proximity_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_SIGNATURE,
|
||||
ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_PROXIMITY
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
|
||||
|
||||
/*
|
||||
rangefinder support
|
||||
*/
|
||||
|
||||
#include <dronecan_msgs.h>
|
||||
|
||||
#ifndef AP_PERIPH_PROBE_CONTINUOUS
|
||||
#define AP_PERIPH_PROBE_CONTINUOUS 0
|
||||
#endif
|
||||
|
||||
/*
|
||||
update CAN rangefinder
|
||||
*/
|
||||
void AP_Periph_FW::can_rangefinder_update(void)
|
||||
{
|
||||
if (rangefinder.get_type(0) == RangeFinder::Type::NONE) {
|
||||
return;
|
||||
}
|
||||
#if AP_PERIPH_PROBE_CONTINUOUS
|
||||
if (rangefinder.num_sensors() == 0) {
|
||||
uint32_t now = AP_HAL::millis();
|
||||
static uint32_t last_probe_ms;
|
||||
if (now - last_probe_ms >= 1000) {
|
||||
last_probe_ms = now;
|
||||
rangefinder.init(ROTATION_NONE);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
uint32_t now = AP_HAL::millis();
|
||||
static uint32_t last_update_ms;
|
||||
if (g.rangefinder_max_rate > 0 &&
|
||||
now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
|
||||
// limit to max rate
|
||||
return;
|
||||
}
|
||||
last_update_ms = now;
|
||||
rangefinder.update();
|
||||
RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE);
|
||||
if (status <= RangeFinder::Status::NoData) {
|
||||
// don't send any data
|
||||
return;
|
||||
}
|
||||
const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE);
|
||||
if (last_sample_ms == sample_ms) {
|
||||
return;
|
||||
}
|
||||
last_sample_ms = sample_ms;
|
||||
|
||||
uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
|
||||
uavcan_equipment_range_sensor_Measurement pkt {};
|
||||
pkt.sensor_id = rangefinder.get_address(0);
|
||||
switch (status) {
|
||||
case RangeFinder::Status::OutOfRangeLow:
|
||||
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
|
||||
break;
|
||||
case RangeFinder::Status::OutOfRangeHigh:
|
||||
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
|
||||
break;
|
||||
case RangeFinder::Status::Good:
|
||||
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
|
||||
break;
|
||||
default:
|
||||
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;
|
||||
break;
|
||||
}
|
||||
switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) {
|
||||
case MAV_DISTANCE_SENSOR_LASER:
|
||||
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
|
||||
break;
|
||||
case MAV_DISTANCE_SENSOR_ULTRASOUND:
|
||||
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR;
|
||||
break;
|
||||
case MAV_DISTANCE_SENSOR_RADAR:
|
||||
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;
|
||||
break;
|
||||
default:
|
||||
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED;
|
||||
break;
|
||||
}
|
||||
|
||||
pkt.range = dist_cm * 0.01;
|
||||
|
||||
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {};
|
||||
uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout());
|
||||
|
||||
canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
|
||||
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
|
||||
CANARD_TRANSFER_PRIORITY_LOW,
|
||||
&buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_RANGEFINDER
|
||||
|
|
@ -58,7 +58,7 @@ int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const
|
|||
uart_num = g.adsb_port;
|
||||
}
|
||||
#endif
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
|
||||
if (uart_num == -1) {
|
||||
uart_num = g.proximity_port;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue