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: add support for sending DShot function command

Browse Source 
add support for dshot on iomcu
add support for updating to dshot iofirmware
This commit is contained in:
bugobliterator 2020-07-11 17:52:05 +05:30 committed by Andrew Tridgell
parent 771f8855b7
commit ad428ac060
5 changed files with 63 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -38,6 +38,7 @@ enum ioevents {
IOEVENT_SET_SAFETY_MASK, IOEVENT_SET_SAFETY_MASK,
IOEVENT_MIXING, IOEVENT_MIXING,
IOEVENT_GPIO, IOEVENT_GPIO,
IOEVENT_SET_OUTPUT_MODE
}; };
// max number of consecutve protocol failures we accept before raising // max number of consecutve protocol failures we accept before raising
@ -230,6 +231,13 @@ void AP_IOMCU::thread_main(void)
} }
mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON); mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON);
if (mask & EVENT_MASK(IOEVENT_SET_OUTPUT_MODE)) {
if (!write_registers(PAGE_SETUP, PAGE_REG_SETUP_OUTPUT_MODE, sizeof(mode_out)/2, (const uint16_t *)&mode_out)) {
event_failed(IOEVENT_SET_OUTPUT_MODE);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) { if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
event_failed(mask); event_failed(mask);
@ -405,7 +413,7 @@ void AP_IOMCU::write_log()
static uint32_t last_io_print; static uint32_t last_io_print;
if (now - last_io_print >= 5000) { if (now - last_io_print >= 5000) {
last_io_print = now; last_io_print = now;
debug("t=%u num=%u mem=%u terr=%u nerr=%u crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%u\n", debug("t=%lu num=%lu mem=%u terr=%lu nerr=%lu crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%lu\n",
now, now,
reg_status.total_pkts, reg_status.total_pkts,
reg_status.freemem, reg_status.freemem,
@ -508,7 +516,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
// wait for the expected number of reply bytes or timeout // wait for the expected number of reply bytes or timeout
if (!uart.wait_timeout(count*2+4, 10)) { if (!uart.wait_timeout(count*2+4, 10)) {
debug("t=%u timeout read page=%u offset=%u count=%u\n", debug("t=%lu timeout read page=%u offset=%u count=%u\n",
AP_HAL::millis(), page, offset, count); AP_HAL::millis(), page, offset, count);
protocol_fail_count++; protocol_fail_count++;
return false; return false;
@ -517,12 +525,12 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
uint8_t *b = (uint8_t *)&pkt; uint8_t *b = (uint8_t *)&pkt;
uint8_t n = uart.available(); uint8_t n = uart.available();
if (n < offsetof(struct IOPacket, regs)) { if (n < offsetof(struct IOPacket, regs)) {
debug("t=%u small pkt %u\n", AP_HAL::millis(), n); debug("t=%lu small pkt %u\n", AP_HAL::millis(), n);
protocol_fail_count++; protocol_fail_count++;
return false; return false;
} }
if (pkt.get_size() != n) { if (pkt.get_size() != n) {
debug("t=%u bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size()); debug("t=%lu bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size());
protocol_fail_count++; protocol_fail_count++;
return false; return false;
} }
@ -532,7 +540,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
pkt.crc = 0; pkt.crc = 0;
uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
if (got_crc != expected_crc) { if (got_crc != expected_crc) {
debug("t=%u bad crc %02x should be %02x n=%u %u/%u/%u\n", debug("t=%lu bad crc %02x should be %02x n=%u %u/%u/%u\n",
AP_HAL::millis(), got_crc, expected_crc, AP_HAL::millis(), got_crc, expected_crc,
n, page, offset, count); n, page, offset, count);
protocol_fail_count++; protocol_fail_count++;
@ -789,6 +797,14 @@ void AP_IOMCU::set_brushed_mode(void)
rate.brushed_enabled = true; rate.brushed_enabled = true;
} }
// set output mode
void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode)
{
mode_out.mask = mask;
mode_out.mode = mode;
trigger_event(IOEVENT_SET_OUTPUT_MODE);
}
// handling of BRD_SAFETYOPTION parameter // handling of BRD_SAFETYOPTION parameter
void AP_IOMCU::update_safety_options(void) void AP_IOMCU::update_safety_options(void)
{ {
@ -837,9 +853,13 @@ bool AP_IOMCU::check_crc(void)
// flash size minus 4k bootloader // flash size minus 4k bootloader
const uint32_t flash_size = 0x10000 - 0x1000; const uint32_t flash_size = 0x10000 - 0x1000;
fw = AP_ROMFS::find_decompress(fw_name, fw_size); if (!AP_BoardConfig::io_dshot()) {
fw = AP_ROMFS::find_decompress(fw_name, fw_size);
} else {
fw = AP_ROMFS::find_decompress(dshot_fw_name, fw_size);
}
if (!fw) { if (!fw) {
DEV_PRINTF("failed to find %s\n", fw_name); DEV_PRINTF("failed to find %s\n", AP_BoardConfig::io_dshot()?dshot_fw_name:fw_name);
return false; return false;
} }
uint32_t crc = crc32_small(0, fw, fw_size); uint32_t crc = crc32_small(0, fw, fw_size);

9
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -98,6 +98,9 @@ public:
// set to brushed mode // set to brushed mode
void set_brushed_mode(void); void set_brushed_mode(void);
// set output mode
void set_output_mode(uint16_t mask, uint16_t mode);
// check if IO is healthy // check if IO is healthy
bool healthy(void); bool healthy(void);
@ -222,6 +225,11 @@ private:
} rate; } rate;
struct page_GPIO GPIO; struct page_GPIO GPIO;
// output mode values
struct {
uint16_t mask;
uint16_t mode;
} mode_out;
// IMU heater duty cycle // IMU heater duty cycle
uint8_t heater_duty_cycle; uint8_t heater_duty_cycle;
@ -248,6 +256,7 @@ private:
// firmware upload // firmware upload
const char *fw_name = "io_firmware.bin"; const char *fw_name = "io_firmware.bin";
const char *dshot_fw_name = "io_firmware_dshot.bin";
const uint8_t *fw; const uint8_t *fw;
uint32_t fw_size; uint32_t fw_size;

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

@ -541,6 +541,11 @@ bool AP_IOMCU_FW::handle_code_write()
} }
break; break;
case PAGE_REG_SETUP_OUTPUT_MODE:
mode_out.mask = rx_io_packet.regs[0];
mode_out.mode = rx_io_packet.regs[1];
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE: case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0]; reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = last_ms; last_heater_ms = last_ms;
@ -760,24 +765,27 @@ void AP_IOMCU_FW::safety_update(void)
*/ */
void AP_IOMCU_FW::rcout_mode_update(void) void AP_IOMCU_FW::rcout_mode_update(void)
{ {
bool use_oneshot = (reg_setup.features & P_SETUP_FEATURES_ONESHOT) != 0; bool use_dshot = mode_out.mode >= AP_HAL::RCOutput::MODE_PWM_DSHOT150 && mode_out.mode <= AP_HAL::RCOutput::MODE_PWM_DSHOT600;
if (use_dshot && !dshot_enabled) {
dshot_enabled = true;
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
}
bool use_oneshot = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT;
if (use_oneshot && !oneshot_enabled) { if (use_oneshot && !oneshot_enabled) {
oneshot_enabled = true; oneshot_enabled = true;
hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_ONESHOT); hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
} }
bool use_brushed = (reg_setup.features & P_SETUP_FEATURES_BRUSHED) != 0; bool use_brushed = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_BRUSHED;
if (use_brushed && !brushed_enabled) { if (use_brushed && !brushed_enabled) {
brushed_enabled = true; brushed_enabled = true;
if (reg_setup.pwm_rates == 0) { // default to 2kHz for all channels for brushed output
// default to 2kHz for all channels for brushed output hal.rcout->set_freq(mode_out.mask, 2000);
reg_setup.pwm_rates = 0xFF;
reg_setup.pwm_altrate = 2000;
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
}
hal.rcout->set_esc_scaling(1000, 2000); hal.rcout->set_esc_scaling(1000, 2000);
hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_BRUSHED); hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate); hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate);
} }
mode_out.mask = 0;
mode_out.mode = 0;
} }
/* /*

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

@ -102,6 +102,12 @@ public:
uint16_t sbus_rate_hz; uint16_t sbus_rate_hz;
} rate; } rate;
// output mode values
struct {
uint16_t mask;
uint16_t mode;
} mode_out;
// MIXER values // MIXER values
struct page_mixing mixing; struct page_mixing mixing;
@ -130,6 +136,7 @@ public:
uint32_t safety_button_counter; uint32_t safety_button_counter;
uint8_t led_counter; uint8_t led_counter;
uint32_t last_loop_ms; uint32_t last_loop_ms;
bool dshot_enabled;
bool oneshot_enabled; bool oneshot_enabled;
bool brushed_enabled; bool brushed_enabled;
thread_t *thread_ctx; thread_t *thread_ctx;

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

@ -78,6 +78,7 @@ enum iopage {
#define PAGE_REG_SETUP_PWM_RATE_MASK 2 #define PAGE_REG_SETUP_PWM_RATE_MASK 2
#define PAGE_REG_SETUP_DEFAULTRATE 3 #define PAGE_REG_SETUP_DEFAULTRATE 3
#define PAGE_REG_SETUP_ALTRATE 4 #define PAGE_REG_SETUP_ALTRATE 4
#define PAGE_REG_SETUP_OUTPUT_MODE 5
#define PAGE_REG_SETUP_REBOOT_BL 10 #define PAGE_REG_SETUP_REBOOT_BL 10
#define PAGE_REG_SETUP_CRC 11 #define PAGE_REG_SETUP_CRC 11
#define PAGE_REG_SETUP_SBUS_RATE 19 #define PAGE_REG_SETUP_SBUS_RATE 19