Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: bdshot for iomcu

Browse Source 
prevent repeated rcout mode sets
add ESC telemetry if compiled in
add infrastructure to support propagating erpm and telemetry from iomcu
add support to propagate bdmask to iomcu
add support for EDT
scale voltage and current correctly when reading EDT data
ensure that telemetry data is reset
reset ESC telemetry data to zero if stale
ESC type and bdmask must be setup before the output mode
This commit is contained in:
Andy Piper 2023-06-23 21:15:14 +01:00 committed by Andrew Tridgell
parent dee0ca2f34
commit 98aeade904
6 changed files with 253 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -113,6 +113,12 @@ void AP_IOMCU::thread_main(void)
uart.begin(1500*1000, 128, 128);
uart.set_unbuffered_writes(true);
AP_BLHeli* blh = AP_BLHeli::get_singleton();
uint16_t erpm_period_ms = 10; // default 100Hz
if (blh && blh->get_telemetry_rate() > 0) {
erpm_period_ms = constrain_int16(1000 / blh->get_telemetry_rate(), 1, 1000);
}
trigger_event(IOEVENT_INIT);
while (!do_shutdown) {
@ -308,6 +314,22 @@ void AP_IOMCU::thread_main(void)
last_servo_read_ms = AP_HAL::millis();
}
if (AP_BoardConfig::io_dshot() && now - last_erpm_read_ms > erpm_period_ms) {
// read erpm at configured rate. A more efficient scheme might be to
// send erpm info back with the response from a PWM send, but that would
// require a reworking of the registers model
read_erpm();
last_erpm_read_ms = AP_HAL::millis();
}
if (AP_BoardConfig::io_dshot() && now - last_telem_read_ms > 100) {
// read dshot telemetry at 10Hz
// needs to be at least 4Hz since each ESC updates at ~1Hz and we
// are reading 4 at a time
read_telem();
last_telem_read_ms = AP_HAL::millis();
}
if (now - last_safety_option_check_ms > 1000) {
update_safety_options();
last_safety_option_check_ms = now;
@ -371,6 +393,66 @@ void AP_IOMCU::read_rc_input()
}
}
/*
read dshot erpm
*/
void AP_IOMCU::read_erpm()
{
uint16_t *r = (uint16_t *)&dshot_erpm;
if (!read_registers(PAGE_RAW_DSHOT_ERPM, 0, sizeof(dshot_erpm)/2, r)) {
return;
}
uint8_t motor_poles = 14;
AP_BLHeli* blh = AP_BLHeli::get_singleton();
if (blh) {
motor_poles = blh->get_motor_poles();
}
for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS/4; i++) {
for (uint8_t j = 0; j < 4; j++) {
const uint8_t esc_id = (i * 4 + j);
if (dshot_erpm.update_mask & 1U<<esc_id) {
update_rpm(esc_id, dshot_erpm.erpm[esc_id] * 200U / motor_poles, dshot_telem[i].error_rate[j] / 100.0);
}
}
}
}
/*
read dshot telemetry
*/
void AP_IOMCU::read_telem()
{
struct page_dshot_telem* telem = &dshot_telem[esc_group];
uint16_t *r = (uint16_t *)telem;
iopage page = PAGE_RAW_DSHOT_TELEM_1_4;
switch (esc_group) {
case 1:
page = PAGE_RAW_DSHOT_TELEM_5_8;
break;
case 2:
page = PAGE_RAW_DSHOT_TELEM_9_12;
break;
case 3:
page = PAGE_RAW_DSHOT_TELEM_13_16;
break;
default:
break;
}
if (!read_registers(page, 0, sizeof(page_dshot_telem)/2, r)) {
return;
}
for (uint i = 0; i<4; i++) {
TelemetryData t {
.temperature_cdeg = int16_t(telem->temperature_cdeg[i]),
.voltage = float(telem->voltage_cvolts[i]) * 0.01,
.current = float(telem->current_camps[i]) * 0.01
};
update_telem_data(esc_group * 4 + i, t, telem->types[i]);
}
esc_group = (esc_group + 1) % 4;
}
/*
read status registers
*/
@ -841,6 +923,13 @@ void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate)
trigger_event(IOEVENT_SET_DSHOT_PERIOD);
}
// set the dshot esc_type
void AP_IOMCU::set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType dshot_esc_type)
{
mode_out.esc_type = uint16_t(dshot_esc_type);
trigger_event(IOEVENT_SET_OUTPUT_MODE);
}
// set output mode
void AP_IOMCU::set_telem_request_mask(uint32_t mask)
{
@ -873,6 +962,13 @@ void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode)
trigger_event(IOEVENT_SET_OUTPUT_MODE);
}
// set output mode
void AP_IOMCU::set_bidir_dshot_mask(uint16_t mask)
{
mode_out.bdmask = mask;
trigger_event(IOEVENT_SET_OUTPUT_MODE);
}
AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const
{
mask = reg_status.rcout_mask;

27
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -13,11 +13,16 @@
#include "iofirmware/ioprotocol.h"
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_HAL/RCOutput.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
typedef uint32_t eventmask_t;
typedef struct ch_thread thread_t;
class AP_IOMCU {
class AP_IOMCU
#ifdef HAL_WITH_ESC_TELEM
: public AP_ESC_Telem_Backend
#endif
{
public:
AP_IOMCU(AP_HAL::UARTDriver &uart);
@ -102,6 +107,9 @@ public:
// set output mode
void set_output_mode(uint16_t mask, uint16_t mode);
// set bi-directional mask
void set_bidir_dshot_mask(uint16_t mask);
// get output mode
AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const;
@ -118,6 +126,9 @@ public:
// set telem request mask
void set_telem_request_mask(uint32_t mask);
// set the dshot esc_type
void set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType dshot_esc_type);
// send a dshot command
void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority);
#endif
@ -192,6 +203,8 @@ private:
uint32_t last_servo_read_ms;
uint32_t last_safety_option_check_ms;
uint32_t last_reg_read_ms;
uint32_t last_erpm_read_ms;
uint32_t last_telem_read_ms;
// last value of safety options
uint16_t last_safety_options = 0xFFFF;
@ -204,6 +217,8 @@ private:
void send_servo_out(void);
void read_rc_input(void);
void read_erpm(void);
void read_telem(void);
void read_servo(void);
void read_status(void);
void discard_input(void);
@ -256,15 +271,17 @@ private:
uint16_t rate;
} dshot_rate;
// bi-directional dshot erpm values
struct page_dshot_erpm dshot_erpm;
struct page_dshot_telem dshot_telem[IOMCU_MAX_CHANNELS/4];
uint8_t esc_group;
// queue of dshot commands that need sending
ObjectBuffer<page_dshot> dshot_command_queue{8};
struct page_GPIO GPIO;
// output mode values
struct {
uint16_t mask;
uint16_t mode;
} mode_out;
struct page_mode_out mode_out;
// IMU heater duty cycle
uint8_t heater_duty_cycle;

97
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -135,8 +135,10 @@ static void dma_tx_end_cb(hal_uart_driver *uart)
(void)uart->usart->DR;
(void)uart->usart->DR;
#ifdef HAL_GPIO_LINE_GPIO108
TOGGLE_PIN_DEBUG(108);
TOGGLE_PIN_DEBUG(108);
#endif
chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END);
}
@ -416,8 +418,9 @@ void AP_IOMCU_FW::update()
// immediate timeout here for lowest latency
eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END, TIME_IMMEDIATE);
#endif
#ifdef HAL_GPIO_LINE_GPIO107
TOGGLE_PIN_DEBUG(107);
#endif
iomcu.reg_status.total_ticks++;
if (mask) {
@ -482,12 +485,20 @@ void AP_IOMCU_FW::update()
// turn amber off
AMBER_SET(0);
}
// update status page at 20Hz
if (now - last_status_ms > 50) {
last_status_ms = now;
page_status_update();
}
#ifdef HAL_WITH_BIDIR_DSHOT
// EDT updates are semt at ~1Hz per ESC, but we want to make sure
// that we don't delay updates unduly so sample at 5Hz
if (now - last_telem_ms > 200) {
last_telem_ms = now;
telem_update();
}
#endif
// run fast loop functions at 1Khz
if (now_us - last_fast_loop_us >= 1000)
{
@ -495,6 +506,9 @@ void AP_IOMCU_FW::update()
heater_update();
rcin_update();
rcin_serial_update();
#ifdef HAL_WITH_BIDIR_DSHOT
erpm_update();
#endif
}
// run remaining functions at 100Hz
@ -521,7 +535,9 @@ void AP_IOMCU_FW::update()
tx_dma_handle->unlock();
}
#endif
#ifdef HAL_GPIO_LINE_GPIO107
TOGGLE_PIN_DEBUG(107);
#endif
}
void AP_IOMCU_FW::pwm_out_update()
@ -603,6 +619,44 @@ void AP_IOMCU_FW::rcin_update()
}
}
#ifdef HAL_WITH_BIDIR_DSHOT
void AP_IOMCU_FW::erpm_update()
{
if (hal.rcout->new_erpm()) {
dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_CHANNELS);
}
}
void AP_IOMCU_FW::telem_update()
{
uint32_t now_ms = AP_HAL::millis();
for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS/4; i++) {
for (uint8_t j = 0; j < 4; j++) {
const uint8_t esc_id = (i * 4 + j);
if (esc_id >= IOMCU_MAX_CHANNELS) {
continue;
}
dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
#if HAL_WITH_ESC_TELEM
const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id);
// if data is stale the set to zero to avoidn phantom data appearing in mavlink
if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) {
dshot_telem[i].voltage_cvolts[j] = 0;
dshot_telem[i].current_camps[j] = 0;
dshot_telem[i].temperature_cdeg[j] = 0;
continue;
}
dshot_telem[i].voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
dshot_telem[i].current_camps[j] = uint16_t(roundf(telem.current * 100));
dshot_telem[i].temperature_cdeg[j] = telem.temperature_cdeg;
dshot_telem[i].types[j] = telem.types;
#endif
}
}
}
#endif
void AP_IOMCU_FW::process_io_packet()
{
iomcu.reg_status.total_pkts++;
@ -701,6 +755,23 @@ bool AP_IOMCU_FW::handle_code_read()
case PAGE_RAW_RCIN:
COPY_PAGE(rc_input);
break;
#ifdef HAL_WITH_BIDIR_DSHOT
case PAGE_RAW_DSHOT_ERPM:
COPY_PAGE(dshot_erpm);
break;
case PAGE_RAW_DSHOT_TELEM_1_4:
COPY_PAGE(dshot_telem[0]);
break;
case PAGE_RAW_DSHOT_TELEM_5_8:
COPY_PAGE(dshot_telem[1]);
break;
case PAGE_RAW_DSHOT_TELEM_9_12:
COPY_PAGE(dshot_telem[2]);
break;
case PAGE_RAW_DSHOT_TELEM_13_16:
COPY_PAGE(dshot_telem[3]);
break;
#endif
case PAGE_STATUS:
COPY_PAGE(reg_status);
break;
@ -727,6 +798,16 @@ bool AP_IOMCU_FW::handle_code_read()
memcpy(tx_io_packet.regs, values, sizeof(uint16_t)*tx_io_packet.count);
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
#ifdef HAL_WITH_BIDIR_DSHOT
switch (rx_io_packet.page) {
case PAGE_RAW_DSHOT_ERPM:
memset(&dshot_erpm, 0, sizeof(dshot_erpm));
break;
default:
break;
}
#endif
return true;
}
@ -814,6 +895,8 @@ bool AP_IOMCU_FW::handle_code_write()
case PAGE_REG_SETUP_OUTPUT_MODE:
mode_out.mask = rx_io_packet.regs[0];
mode_out.mode = rx_io_packet.regs[1];
mode_out.bdmask = rx_io_packet.regs[2];
mode_out.esc_type = rx_io_packet.regs[3];
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
@ -1066,7 +1149,9 @@ void AP_IOMCU_FW::rcout_config_update(void)
}
// see if there is anything to do, we only support setting the mode for a particular channel once
if ((last_output_mode_mask & ~mode_out.mask) == mode_out.mask) {
if ((last_output_mode_mask & mode_out.mask) == mode_out.mask
&& (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask
&& last_output_esc_type == mode_out.esc_type) {
return;
}
@ -1075,11 +1160,17 @@ void AP_IOMCU_FW::rcout_config_update(void)
case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
#if defined(STM32F103xB) || defined(STM32F103x8)
case AP_HAL::RCOutput::MODE_PWM_DSHOT600:
#endif
#ifdef HAL_WITH_BIDIR_DSHOT
hal.rcout->set_bidir_dshot_mask(mode_out.bdmask);
hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
#endif
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
// enabling dshot changes the memory allocation
reg_status.freemem = hal.util->available_memory();
last_output_mode_mask |= mode_out.mask;
last_output_bdmask |= mode_out.bdmask;
last_output_esc_type = mode_out.esc_type;
break;
case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:

18
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -4,6 +4,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_RCProtocol/AP_RCProtocol.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include "hal.h"
#include "ch.h"
@ -29,6 +30,8 @@ public:
void pwm_out_update();
void heater_update();
void rcin_update();
void erpm_update();
void telem_update();
bool handle_code_write();
bool handle_code_read();
@ -112,12 +115,11 @@ public:
} rate;
// output mode values
struct {
uint16_t mask;
uint16_t mode;
} mode_out;
struct page_mode_out mode_out;
uint16_t last_output_mode_mask;
uint16_t last_output_bdmask;
uint16_t last_output_esc_type;
// MIXER values
struct page_mixing mixing;
@ -135,6 +137,14 @@ public:
void tx_dma_deallocate(ChibiOS::Shared_DMA *ctx);
ChibiOS::Shared_DMA* tx_dma_handle;
#endif
#ifdef HAL_WITH_BIDIR_DSHOT
struct page_dshot_erpm dshot_erpm;
struct page_dshot_telem dshot_telem[IOMCU_MAX_CHANNELS/4];
uint32_t last_telem_ms;
#if HAL_WITH_ESC_TELEM
AP_ESC_Telem esc_telem;
#endif
#endif
// true when override channel active

26
libraries/AP_IOMCU/iofirmware/ioprotocol.h
View File

@ -58,6 +58,11 @@ enum iopage {
PAGE_MIXING = 200,
PAGE_GPIO = 201,
PAGE_DSHOT = 202,
PAGE_RAW_DSHOT_ERPM = 203,
PAGE_RAW_DSHOT_TELEM_1_4 = 204,
PAGE_RAW_DSHOT_TELEM_5_8 = 205,
PAGE_RAW_DSHOT_TELEM_9_12 = 206,
PAGE_RAW_DSHOT_TELEM_13_16 = 207,
};
// setup page registers
@ -184,6 +189,13 @@ struct __attribute__((packed, aligned(2))) page_GPIO {
uint8_t output_mask;
};
struct __attribute__((packed, aligned(2))) page_mode_out {
uint16_t mask;
uint16_t mode;
uint16_t bdmask;
uint16_t esc_type;
};
struct __attribute__((packed, aligned(2))) page_dshot {
uint16_t telem_mask;
uint8_t command;
@ -192,3 +204,17 @@ struct __attribute__((packed, aligned(2))) page_dshot {
uint8_t repeat_count;
uint8_t priority;
};
struct __attribute__((packed, aligned(2))) page_dshot_erpm {
uint16_t erpm[IOMCU_MAX_CHANNELS];
uint32_t update_mask;
};
// separate telemetry packet because (a) it's too big otherwise and (b) slower update rate
struct __attribute__((packed, aligned(2))) page_dshot_telem {
uint16_t error_rate[4]; // as a centi-percentage
uint16_t voltage_cvolts[4];
uint16_t current_camps[4];
uint16_t temperature_cdeg[4];
uint16_t types[4];
};

1
libraries/AP_IOMCU/iofirmware/wscript
View File

@ -12,6 +12,7 @@ def build(bld):
'AP_Math',
'AP_RCProtocol',
'AP_BoardConfig',
'AP_ESC_Telem',
'AP_SBusOut'
],
exclude_src=[