diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 9045193bb2..2cf81c9165 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -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<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; diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 794844a950..041c565f48 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -13,11 +13,16 @@ #include "iofirmware/ioprotocol.h" #include #include +#include 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 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; diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 3d1cce6f62..2ff7db713b 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -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: diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 62c8b12565..16227f39f7 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -4,6 +4,7 @@ #include #include #include +#include #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 diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 21b0a4263a..44c10d5054 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -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]; +}; diff --git a/libraries/AP_IOMCU/iofirmware/wscript b/libraries/AP_IOMCU/iofirmware/wscript index 7e051d72cb..445b95c0e1 100644 --- a/libraries/AP_IOMCU/iofirmware/wscript +++ b/libraries/AP_IOMCU/iofirmware/wscript @@ -12,6 +12,7 @@ def build(bld): 'AP_Math', 'AP_RCProtocol', 'AP_BoardConfig', + 'AP_ESC_Telem', 'AP_SBusOut' ], exclude_src=[