2017-12-26 19:03:21 -04:00
|
|
|
/*
|
|
|
|
implement protocol for controlling an IO microcontroller
|
|
|
|
|
|
|
|
For bootstrapping this will initially implement the px4io protocol,
|
|
|
|
but will later move to an ArduPilot specific protocol
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "AP_IOMCU.h"
|
2018-01-02 01:47:42 -04:00
|
|
|
|
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_Math/crc.h>
|
2018-04-13 03:17:08 -03:00
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
2018-04-14 08:09:11 -03:00
|
|
|
#include <AP_ROMFS/AP_ROMFS.h>
|
2018-10-30 21:07:47 -03:00
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
|
|
#include <RC_Channel/RC_Channel.h>
|
2018-11-05 01:44:20 -04:00
|
|
|
#include <AP_RCProtocol/AP_RCProtocol.h>
|
2019-04-21 23:54:00 -03:00
|
|
|
#include <AP_InternalError/AP_InternalError.h>
|
2019-08-13 21:07:48 -03:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2023-02-10 19:44:09 -04:00
|
|
|
#include <AP_Arming/AP_Arming.h>
|
2024-02-01 19:53:49 -04:00
|
|
|
#include <AP_BLHeli/AP_BLHeli.h>
|
2022-02-21 03:38:21 -04:00
|
|
|
#include <ch.h>
|
2017-12-26 19:03:21 -04:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
|
2018-05-11 03:31:04 -03:00
|
|
|
// pending IO events to send, used as an event mask
|
|
|
|
enum ioevents {
|
|
|
|
IOEVENT_INIT=1,
|
|
|
|
IOEVENT_SEND_PWM_OUT,
|
|
|
|
IOEVENT_FORCE_SAFETY_OFF,
|
|
|
|
IOEVENT_FORCE_SAFETY_ON,
|
|
|
|
IOEVENT_SET_ONESHOT_ON,
|
2018-07-12 23:28:43 -03:00
|
|
|
IOEVENT_SET_BRUSHED_ON,
|
2018-05-11 03:31:04 -03:00
|
|
|
IOEVENT_SET_RATES,
|
|
|
|
IOEVENT_ENABLE_SBUS,
|
|
|
|
IOEVENT_SET_HEATER_TARGET,
|
|
|
|
IOEVENT_SET_DEFAULT_RATE,
|
|
|
|
IOEVENT_SET_SAFETY_MASK,
|
2021-09-20 10:44:12 -03:00
|
|
|
IOEVENT_MIXING,
|
|
|
|
IOEVENT_GPIO,
|
2023-05-24 17:43:17 -03:00
|
|
|
IOEVENT_SET_OUTPUT_MODE,
|
|
|
|
IOEVENT_SET_DSHOT_PERIOD,
|
|
|
|
IOEVENT_SET_CHANNEL_MASK,
|
|
|
|
IOEVENT_DSHOT,
|
2018-05-11 03:31:04 -03:00
|
|
|
};
|
|
|
|
|
2019-04-23 22:34:20 -03:00
|
|
|
// max number of consecutve protocol failures we accept before raising
|
|
|
|
// an error
|
|
|
|
#define IOMCU_MAX_REPEATED_FAILURES 20
|
|
|
|
|
2023-03-01 00:05:02 -04:00
|
|
|
#ifndef AP_IOMCU_FORCE_ENABLE_HEATER
|
|
|
|
#define AP_IOMCU_FORCE_ENABLE_HEATER 0
|
|
|
|
#endif
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) :
|
|
|
|
uart(_uart)
|
2019-11-01 23:32:48 -03:00
|
|
|
{
|
|
|
|
singleton = this;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
|
2023-06-17 19:26:56 -03:00
|
|
|
#define IOMCU_DEBUG_ENABLE 0
|
2019-08-13 21:07:48 -03:00
|
|
|
|
|
|
|
#if IOMCU_DEBUG_ENABLE
|
2021-02-23 00:08:38 -04:00
|
|
|
#include <stdio.h>
|
2018-10-30 21:07:47 -03:00
|
|
|
#define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
|
|
#else
|
|
|
|
#define debug(fmt, args ...)
|
|
|
|
#endif
|
|
|
|
|
2019-11-01 23:32:48 -03:00
|
|
|
AP_IOMCU *AP_IOMCU::singleton;
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
initialise library, starting thread
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::init(void)
|
|
|
|
{
|
2018-04-14 08:09:11 -03:00
|
|
|
// uart runs at 1.5MBit
|
2020-01-17 00:59:00 -04:00
|
|
|
uart.begin(1500*1000, 128, 128);
|
2018-04-14 08:09:11 -03:00
|
|
|
uart.set_unbuffered_writes(true);
|
|
|
|
|
2023-11-13 15:58:36 -04:00
|
|
|
#if IOMCU_DEBUG_ENABLE
|
|
|
|
crc_is_ok = true;
|
|
|
|
#else
|
2019-02-10 14:06:22 -04:00
|
|
|
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
|
2019-05-02 21:56:41 -03:00
|
|
|
if ((!boardconfig || boardconfig->io_enabled() == 1) && !hal.util->was_watchdog_reset()) {
|
2018-05-15 23:53:23 -03:00
|
|
|
check_crc();
|
2018-10-30 21:07:47 -03:00
|
|
|
} else {
|
|
|
|
crc_is_ok = true;
|
2018-05-15 23:53:23 -03:00
|
|
|
}
|
2023-11-13 15:58:36 -04:00
|
|
|
#endif
|
2018-07-06 21:39:24 -03:00
|
|
|
|
|
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
|
|
|
|
1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
|
2017-12-27 03:24:15 -04:00
|
|
|
AP_HAL::panic("Unable to allocate IOMCU thread");
|
|
|
|
}
|
2018-10-30 21:07:47 -03:00
|
|
|
initialised = true;
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
2018-01-02 02:59:20 -04:00
|
|
|
/*
|
|
|
|
handle event failure
|
|
|
|
*/
|
2021-02-23 17:47:24 -04:00
|
|
|
void AP_IOMCU::event_failed(uint32_t event_mask)
|
2018-01-02 02:59:20 -04:00
|
|
|
{
|
|
|
|
// wait 0.5ms then retry
|
|
|
|
hal.scheduler->delay_microseconds(500);
|
2021-02-23 17:47:24 -04:00
|
|
|
chEvtSignal(thread_ctx, event_mask);
|
2018-01-02 02:59:20 -04:00
|
|
|
}
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
main IO thread loop
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::thread_main(void)
|
2017-12-26 19:03:21 -04:00
|
|
|
{
|
2018-01-07 23:52:11 -04:00
|
|
|
thread_ctx = chThdGetSelfX();
|
2018-11-03 03:27:10 -03:00
|
|
|
chEvtSignal(thread_ctx, initial_event_mask);
|
2018-04-14 08:09:11 -03:00
|
|
|
|
2020-01-17 00:59:00 -04:00
|
|
|
uart.begin(1500*1000, 128, 128);
|
2018-01-21 16:29:15 -04:00
|
|
|
uart.set_unbuffered_writes(true);
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2024-03-17 15:42:01 -03:00
|
|
|
#if HAL_WITH_IO_MCU_BIDIR_DSHOT
|
2023-06-23 17:15:14 -03:00
|
|
|
uint16_t erpm_period_ms = 10; // default 100Hz
|
2024-03-17 15:42:01 -03:00
|
|
|
#if HAVE_AP_BLHELI_SUPPORT
|
|
|
|
AP_BLHeli* blh = AP_BLHeli::get_singleton();
|
2023-06-23 17:15:14 -03:00
|
|
|
if (blh && blh->get_telemetry_rate() > 0) {
|
|
|
|
erpm_period_ms = constrain_int16(1000 / blh->get_telemetry_rate(), 1, 1000);
|
|
|
|
}
|
2024-03-17 15:42:01 -03:00
|
|
|
#endif
|
2023-11-16 13:44:55 -04:00
|
|
|
#endif
|
2018-01-02 02:59:20 -04:00
|
|
|
trigger_event(IOEVENT_INIT);
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2018-10-29 18:50:59 -03:00
|
|
|
while (!do_shutdown) {
|
2023-02-10 21:40:26 -04:00
|
|
|
// check if we have lost contact with the IOMCU
|
|
|
|
const uint32_t now_ms = AP_HAL::millis();
|
2023-11-22 05:23:21 -04:00
|
|
|
if (last_reg_access_ms != 0 && now_ms - last_reg_access_ms > 1000) {
|
2023-02-10 21:40:26 -04:00
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
|
2023-11-22 05:23:21 -04:00
|
|
|
last_reg_access_ms = 0;
|
2023-02-10 21:40:26 -04:00
|
|
|
}
|
|
|
|
|
2018-06-02 12:59:33 -03:00
|
|
|
eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10));
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
// check for pending IO events
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) {
|
|
|
|
send_servo_out();
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SEND_PWM_OUT);
|
2017-12-27 03:24:15 -04:00
|
|
|
|
2018-01-02 02:59:20 -04:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_INIT)) {
|
2018-10-30 21:07:47 -03:00
|
|
|
// get protocol version
|
2018-11-01 03:39:24 -03:00
|
|
|
if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-10-30 21:07:47 -03:00
|
|
|
continue;
|
|
|
|
}
|
2018-11-01 03:39:24 -03:00
|
|
|
is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION &&
|
|
|
|
config.protocol_version2 == IOMCU_PROTOCOL_VERSION2);
|
2018-10-30 21:07:47 -03:00
|
|
|
|
2023-06-17 19:26:56 -03:00
|
|
|
DEV_PRINTF("IOMCU: 0x%lx\n", config.mcuid);
|
|
|
|
|
2024-08-13 09:59:08 -03:00
|
|
|
// set IO_ARM_OK and clear FMU_ARMED
|
|
|
|
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, P_SETUP_ARMING_FMU_ARMED,
|
2018-01-02 02:59:20 -04:00
|
|
|
P_SETUP_ARMING_IO_ARM_OK |
|
|
|
|
P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-02 02:59:20 -04:00
|
|
|
continue;
|
|
|
|
}
|
2023-03-01 00:05:02 -04:00
|
|
|
|
|
|
|
#if AP_IOMCU_FORCE_ENABLE_HEATER
|
|
|
|
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0,
|
|
|
|
P_SETUP_FEATURES_HEATER)) {
|
|
|
|
event_failed(mask);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
#endif
|
2018-01-02 02:59:20 -04:00
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_INIT);
|
2018-01-02 02:59:20 -04:00
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_MIXING)) {
|
|
|
|
if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-10-30 21:07:47 -03:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_MIXING);
|
2018-10-30 21:07:47 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
|
2018-01-02 02:59:20 -04:00
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-02 02:59:20 -04:00
|
|
|
continue;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF);
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
|
2018-01-02 02:59:20 -04:00
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-02 02:59:20 -04:00
|
|
|
continue;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_ON);
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 05:15:55 -04:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
|
2018-01-02 02:59:20 -04:00
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq) ||
|
|
|
|
!write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-02 02:59:20 -04:00
|
|
|
continue;
|
|
|
|
}
|
2017-12-27 05:15:55 -04:00
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_RATES);
|
2017-12-31 22:28:59 -04:00
|
|
|
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) {
|
2018-01-02 02:59:20 -04:00
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, rate.sbus_rate_hz) ||
|
|
|
|
!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0,
|
|
|
|
P_SETUP_FEATURES_SBUS1_OUT)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2019-10-20 10:47:14 -03:00
|
|
|
continue;
|
2018-01-02 02:59:20 -04:00
|
|
|
}
|
2017-12-31 22:28:59 -04:00
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_ENABLE_SBUS);
|
2018-01-03 02:25:30 -04:00
|
|
|
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET)) {
|
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-03 02:25:30 -04:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_HEATER_TARGET);
|
2018-01-12 23:52:29 -04:00
|
|
|
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) {
|
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-12 23:52:29 -04:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_DEFAULT_RATE);
|
2018-01-16 01:37:14 -04:00
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_DSHOT_PERIOD)) {
|
|
|
|
if (!write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSHOT_PERIOD, sizeof(dshot_rate)/2, (const uint16_t *)&dshot_rate)) {
|
|
|
|
event_failed(mask);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_DSHOT_PERIOD);
|
|
|
|
|
2018-01-16 01:37:14 -04:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
|
|
|
|
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-01-16 01:37:14 -04:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_ONESHOT_ON);
|
2018-04-14 00:54:04 -03:00
|
|
|
|
2018-07-12 23:28:43 -03:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) {
|
|
|
|
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-07-12 23:28:43 -03:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON);
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2020-07-11 09:22:05 -03:00
|
|
|
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)) {
|
2023-08-14 04:43:33 -03:00
|
|
|
event_failed(mask);
|
2020-07-11 09:22:05 -03:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2023-05-24 17:43:17 -03:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_OUTPUT_MODE);
|
|
|
|
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_CHANNEL_MASK)) {
|
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_CHANNEL_MASK, pwm_out.channel_mask)) {
|
|
|
|
event_failed(mask);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_CHANNEL_MASK);
|
2020-07-11 09:22:05 -03:00
|
|
|
|
2018-04-14 00:54:04 -03:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
|
|
|
|
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
|
2021-02-23 17:47:24 -04:00
|
|
|
event_failed(mask);
|
2018-04-14 00:54:04 -03:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 17:47:24 -04:00
|
|
|
mask &= ~EVENT_MASK(IOEVENT_SET_SAFETY_MASK);
|
2018-10-30 21:07:47 -03:00
|
|
|
|
2021-09-20 10:44:12 -03:00
|
|
|
if (is_chibios_backend) {
|
|
|
|
if (mask & EVENT_MASK(IOEVENT_GPIO)) {
|
|
|
|
if (!write_registers(PAGE_GPIO, 0, sizeof(GPIO)/sizeof(uint16_t), (const uint16_t*)&GPIO)) {
|
|
|
|
event_failed(mask);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
mask &= ~EVENT_MASK(IOEVENT_GPIO);
|
|
|
|
}
|
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
if (mask & EVENT_MASK(IOEVENT_DSHOT)) {
|
2023-07-10 06:32:29 -03:00
|
|
|
page_dshot dshot;
|
|
|
|
if (!dshot_command_queue.pop(dshot) || !write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) {
|
2023-05-24 17:43:17 -03:00
|
|
|
event_failed(mask);
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
mask &= ~EVENT_MASK(IOEVENT_DSHOT);
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
// check for regular timed events
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
if (now - last_rc_read_ms > 20) {
|
|
|
|
// read RC input at 50Hz
|
|
|
|
read_rc_input();
|
|
|
|
last_rc_read_ms = AP_HAL::millis();
|
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
if (now - last_status_read_ms > 50) {
|
|
|
|
// read status at 20Hz
|
|
|
|
read_status();
|
|
|
|
last_status_read_ms = AP_HAL::millis();
|
2022-08-07 03:37:31 -03:00
|
|
|
write_log();
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
if (now - last_servo_read_ms > 50) {
|
|
|
|
// read servo out at 20Hz
|
|
|
|
read_servo();
|
|
|
|
last_servo_read_ms = AP_HAL::millis();
|
|
|
|
}
|
2024-03-17 15:42:01 -03:00
|
|
|
#if HAL_WITH_IO_MCU_BIDIR_DSHOT
|
2023-06-23 17:15:14 -03:00
|
|
|
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();
|
|
|
|
}
|
2023-11-16 13:44:55 -04:00
|
|
|
#endif
|
2024-08-13 09:59:08 -03:00
|
|
|
// update options at the same rate that the iomcu updates the state
|
|
|
|
if (now - last_safety_option_check_ms > 100) {
|
2018-04-13 03:17:08 -03:00
|
|
|
update_safety_options();
|
|
|
|
last_safety_option_check_ms = now;
|
|
|
|
}
|
2018-04-14 00:54:04 -03:00
|
|
|
|
2018-09-12 17:22:26 -03:00
|
|
|
// update failsafe pwm
|
|
|
|
if (pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent) {
|
|
|
|
uint8_t set = pwm_out.failsafe_pwm_set;
|
2024-04-16 13:56:58 -03:00
|
|
|
if (write_registers(PAGE_FAILSAFE_PWM, 0, IOMCU_MAX_RC_CHANNELS, pwm_out.failsafe_pwm)) {
|
2018-09-12 17:22:26 -03:00
|
|
|
pwm_out.failsafe_pwm_sent = set;
|
|
|
|
}
|
2018-04-14 00:54:04 -03:00
|
|
|
}
|
2020-08-12 23:28:47 -03:00
|
|
|
|
|
|
|
send_rc_protocols();
|
2017-12-26 19:03:21 -04:00
|
|
|
}
|
2018-10-29 18:50:59 -03:00
|
|
|
done_shutdown = true;
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
2017-12-26 19:03:21 -04:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
send servo output data
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::send_servo_out()
|
|
|
|
{
|
2018-09-12 17:22:26 -03:00
|
|
|
#if 0
|
|
|
|
// simple method to test IO failsafe
|
|
|
|
if (AP_HAL::millis() > 30000) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
#endif
|
2017-12-27 03:24:15 -04:00
|
|
|
if (pwm_out.num_channels > 0) {
|
2018-01-16 06:21:59 -04:00
|
|
|
uint8_t n = pwm_out.num_channels;
|
|
|
|
if (rate.sbus_rate_hz == 0) {
|
|
|
|
n = MIN(n, 8);
|
2018-10-30 21:07:47 -03:00
|
|
|
} else {
|
2024-04-16 13:56:58 -03:00
|
|
|
n = MIN(n, IOMCU_MAX_RC_CHANNELS);
|
2018-01-16 06:21:59 -04:00
|
|
|
}
|
|
|
|
uint32_t now = AP_HAL::micros();
|
2023-05-24 17:43:17 -03:00
|
|
|
if (now - last_servo_out_us >= 2000 || AP_BoardConfig::io_dshot()) {
|
|
|
|
// don't send data at more than 500Hz except when using dshot which is more timing sensitive
|
2018-09-12 17:22:26 -03:00
|
|
|
if (write_registers(PAGE_DIRECT_PWM, 0, n, pwm_out.pwm)) {
|
|
|
|
last_servo_out_us = now;
|
|
|
|
}
|
2018-01-16 06:21:59 -04:00
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
}
|
2017-12-26 19:03:21 -04:00
|
|
|
}
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
read RC input
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::read_rc_input()
|
|
|
|
{
|
2019-08-13 21:07:48 -03:00
|
|
|
uint16_t *r = (uint16_t *)&rc_input;
|
|
|
|
if (!read_registers(PAGE_RAW_RCIN, 0, sizeof(rc_input)/2, r)) {
|
2019-08-13 04:59:18 -03:00
|
|
|
return;
|
|
|
|
}
|
2023-06-11 02:58:15 -03:00
|
|
|
if (rc_input.flags_failsafe && rc().option_is_enabled(RC_Channels::Option::IGNORE_FAILSAFE)) {
|
2020-08-23 19:48:36 -03:00
|
|
|
rc_input.flags_failsafe = false;
|
|
|
|
}
|
2018-07-18 16:57:22 -03:00
|
|
|
if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) {
|
2019-08-13 21:07:48 -03:00
|
|
|
rc_last_input_ms = AP_HAL::millis();
|
2017-12-31 22:28:59 -04:00
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
2017-12-26 19:03:21 -04:00
|
|
|
|
2024-03-17 15:42:01 -03:00
|
|
|
#if HAL_WITH_IO_MCU_BIDIR_DSHOT
|
2023-06-23 17:15:14 -03:00
|
|
|
/*
|
|
|
|
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;
|
2024-03-17 15:42:01 -03:00
|
|
|
#if HAVE_AP_BLHELI_SUPPORT
|
2023-06-23 17:15:14 -03:00
|
|
|
AP_BLHeli* blh = AP_BLHeli::get_singleton();
|
|
|
|
if (blh) {
|
|
|
|
motor_poles = blh->get_motor_poles();
|
|
|
|
}
|
2024-03-17 15:42:01 -03:00
|
|
|
#endif
|
2023-11-13 15:58:36 -04:00
|
|
|
for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) {
|
2023-06-23 17:15:14 -03:00
|
|
|
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) {
|
2023-11-13 15:58:36 -04:00
|
|
|
#if IOMCU_MAX_TELEM_CHANNELS > 4
|
2023-06-23 17:15:14 -03:00
|
|
|
case 1:
|
|
|
|
page = PAGE_RAW_DSHOT_TELEM_5_8;
|
|
|
|
break;
|
2023-11-13 15:58:36 -04:00
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
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,
|
2024-05-13 20:16:23 -03:00
|
|
|
.current = float(telem->current_camps[i]) * 0.01,
|
|
|
|
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
|
|
|
|
.edt2_status = telem->edt2_status[i],
|
|
|
|
.edt2_stress = telem->edt2_stress[i],
|
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
};
|
|
|
|
update_telem_data(esc_group * 4 + i, t, telem->types[i]);
|
|
|
|
}
|
2023-11-13 15:58:36 -04:00
|
|
|
esc_group = (esc_group + 1) % (IOMCU_MAX_TELEM_CHANNELS / 4);
|
2023-06-23 17:15:14 -03:00
|
|
|
}
|
2023-11-16 13:44:55 -04:00
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
read status registers
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::read_status()
|
2017-12-26 19:03:21 -04:00
|
|
|
{
|
2017-12-31 22:28:59 -04:00
|
|
|
uint16_t *r = (uint16_t *)®_status;
|
2019-08-13 04:59:18 -03:00
|
|
|
if (!read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r)) {
|
2020-01-17 02:36:01 -04:00
|
|
|
read_status_errors++;
|
2023-02-10 21:40:26 -04:00
|
|
|
if (read_status_errors == 20 && last_iocmu_timestamp_ms != 0) {
|
|
|
|
// the IOMCU has stopped responding to status requests
|
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
|
|
|
|
}
|
2019-08-13 04:59:18 -03:00
|
|
|
return;
|
|
|
|
}
|
2020-01-17 02:36:01 -04:00
|
|
|
if (read_status_ok == 0) {
|
|
|
|
// reset error count on first good read
|
|
|
|
read_status_errors = 0;
|
|
|
|
}
|
|
|
|
read_status_ok++;
|
2018-09-03 23:16:19 -03:00
|
|
|
|
2019-04-23 22:34:20 -03:00
|
|
|
check_iomcu_reset();
|
|
|
|
|
2018-09-03 23:16:19 -03:00
|
|
|
if (reg_status.flag_safety_off == 0) {
|
|
|
|
// if the IOMCU is indicating that safety is on, then force a
|
|
|
|
// re-check of the safety options. This copes with a IOMCU reset
|
|
|
|
last_safety_options = 0xFFFF;
|
|
|
|
|
|
|
|
// also check if the safety should be definately off.
|
2019-02-10 14:06:22 -04:00
|
|
|
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
|
2018-09-03 23:16:19 -03:00
|
|
|
if (!boardconfig) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
uint16_t options = boardconfig->get_safety_button_options();
|
|
|
|
if (safety_forced_off && (options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON) == 0) {
|
|
|
|
// the safety has been forced off, and the user has asked
|
|
|
|
// that the button can never be used, so there should be
|
|
|
|
// no way for the safety to be on except a IOMCU
|
|
|
|
// reboot. Force safety off again
|
|
|
|
force_safety_off();
|
|
|
|
}
|
|
|
|
}
|
2022-08-07 03:36:42 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU::write_log()
|
|
|
|
{
|
2019-08-13 21:07:48 -03:00
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
if (now - last_log_ms >= 1000U) {
|
|
|
|
last_log_ms = now;
|
2023-07-13 21:58:06 -03:00
|
|
|
#if HAL_LOGGING_ENABLED
|
2019-11-14 00:59:33 -04:00
|
|
|
if (AP_Logger::get_singleton()) {
|
2020-04-06 23:55:39 -03:00
|
|
|
// @LoggerMessage: IOMC
|
|
|
|
// @Description: IOMCU diagnostic information
|
|
|
|
// @Field: TimeUS: Time since system startup
|
2022-08-07 03:55:35 -03:00
|
|
|
// @Field: RSErr: Status Read error count (zeroed on successful read)
|
2020-04-06 23:55:39 -03:00
|
|
|
// @Field: Mem: Free memory
|
|
|
|
// @Field: TS: IOMCU uptime
|
|
|
|
// @Field: NPkt: Number of packets received by IOMCU
|
|
|
|
// @Field: Nerr: Protocol failures on MCU side
|
|
|
|
// @Field: Nerr2: Reported number of failures on IOMCU side
|
|
|
|
// @Field: NDel: Number of delayed packets received by MCU
|
2022-08-07 03:55:35 -03:00
|
|
|
AP::logger().WriteStreaming("IOMC", "TimeUS,RSErr,Mem,TS,NPkt,Nerr,Nerr2,NDel", "QHHIIIII",
|
2019-11-14 00:59:33 -04:00
|
|
|
AP_HAL::micros64(),
|
2022-08-07 03:55:35 -03:00
|
|
|
read_status_errors,
|
2019-11-14 00:59:33 -04:00
|
|
|
reg_status.freemem,
|
|
|
|
reg_status.timestamp_ms,
|
|
|
|
reg_status.total_pkts,
|
|
|
|
total_errors,
|
|
|
|
reg_status.num_errors,
|
|
|
|
num_delayed);
|
|
|
|
}
|
2023-07-13 21:58:06 -03:00
|
|
|
#endif // HAL_LOGGING_ENABLED
|
2019-08-13 21:07:48 -03:00
|
|
|
#if IOMCU_DEBUG_ENABLE
|
2019-08-13 21:56:51 -03:00
|
|
|
static uint32_t last_io_print;
|
|
|
|
if (now - last_io_print >= 5000) {
|
|
|
|
last_io_print = now;
|
2023-05-24 17:43:17 -03:00
|
|
|
debug("t=%lu num=%lu mem=%u mstack=%u pstack=%u terr=%lu nerr=%lu crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%lu\n",
|
2019-08-13 21:56:51 -03:00
|
|
|
now,
|
|
|
|
reg_status.total_pkts,
|
2021-02-23 00:08:38 -04:00
|
|
|
reg_status.freemem,
|
2023-05-24 17:43:17 -03:00
|
|
|
reg_status.freemstack,
|
|
|
|
reg_status.freepstack,
|
2019-08-13 21:56:51 -03:00
|
|
|
total_errors,
|
|
|
|
reg_status.num_errors,
|
2019-08-13 21:07:48 -03:00
|
|
|
reg_status.err_crc,
|
|
|
|
reg_status.err_bad_opcode,
|
|
|
|
reg_status.err_read,
|
|
|
|
reg_status.err_write,
|
2019-08-13 21:56:51 -03:00
|
|
|
reg_status.err_uart,
|
|
|
|
num_delayed);
|
2019-08-13 21:07:48 -03:00
|
|
|
}
|
|
|
|
#endif // IOMCU_DEBUG_ENABLE
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
2017-12-26 19:03:21 -04:00
|
|
|
|
2023-07-13 21:58:06 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
/*
|
|
|
|
read servo output values
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::read_servo()
|
|
|
|
{
|
|
|
|
if (pwm_out.num_channels > 0) {
|
|
|
|
read_registers(PAGE_SERVOS, 0, pwm_out.num_channels, pwm_in.pwm);
|
2017-12-26 19:03:21 -04:00
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
discard any pending input
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::discard_input(void)
|
|
|
|
{
|
2020-05-22 21:24:32 -03:00
|
|
|
uart.discard_input();
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
2019-08-13 21:07:48 -03:00
|
|
|
/*
|
|
|
|
write a packet, retrying as needed
|
|
|
|
*/
|
|
|
|
size_t AP_IOMCU::write_wait(const uint8_t *pkt, uint8_t len)
|
|
|
|
{
|
2019-08-13 21:56:51 -03:00
|
|
|
uint8_t wait_count = 5;
|
2019-08-13 21:07:48 -03:00
|
|
|
size_t ret;
|
|
|
|
do {
|
|
|
|
ret = uart.write(pkt, len);
|
|
|
|
if (ret == 0) {
|
|
|
|
hal.scheduler->delay_microseconds(100);
|
2019-08-13 21:56:51 -03:00
|
|
|
num_delayed++;
|
2019-08-13 21:07:48 -03:00
|
|
|
}
|
|
|
|
} while (ret == 0 && wait_count--);
|
|
|
|
return ret;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
read count 16 bit registers
|
|
|
|
*/
|
|
|
|
bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs)
|
|
|
|
{
|
2018-10-30 21:07:47 -03:00
|
|
|
while (count > PKT_MAX_REGS) {
|
|
|
|
if (!read_registers(page, offset, PKT_MAX_REGS, regs)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
offset += PKT_MAX_REGS;
|
|
|
|
count -= PKT_MAX_REGS;
|
|
|
|
regs += PKT_MAX_REGS;
|
|
|
|
}
|
|
|
|
|
2017-12-26 19:03:21 -04:00
|
|
|
IOPacket pkt;
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
discard_input();
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
memset(&pkt.regs[0], 0, count*2);
|
2017-12-26 19:03:21 -04:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
pkt.code = CODE_READ;
|
|
|
|
pkt.count = count;
|
|
|
|
pkt.page = page;
|
|
|
|
pkt.offset = offset;
|
|
|
|
pkt.crc = 0;
|
2018-10-30 21:24:51 -03:00
|
|
|
|
|
|
|
uint8_t pkt_size = pkt.get_size();
|
2018-11-01 03:39:24 -03:00
|
|
|
if (is_chibios_backend) {
|
2019-08-13 21:07:48 -03:00
|
|
|
/*
|
|
|
|
the original read protocol is a bit strange, as it
|
|
|
|
unnecessarily sends the same size packet that it expects to
|
|
|
|
receive. This means reading a large number of registers
|
|
|
|
wastes a lot of serial bandwidth. We avoid this overhead
|
|
|
|
when we know we are talking to a ChibiOS backend
|
|
|
|
*/
|
2018-10-30 21:24:51 -03:00
|
|
|
pkt_size = 4;
|
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2018-10-30 21:24:51 -03:00
|
|
|
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
|
2019-08-13 21:07:48 -03:00
|
|
|
|
|
|
|
size_t ret = write_wait((uint8_t *)&pkt, pkt_size);
|
|
|
|
|
|
|
|
if (ret != pkt_size) {
|
|
|
|
debug("write failed1 %u %u %u\n", unsigned(pkt_size), page, offset);
|
2018-10-30 21:07:47 -03:00
|
|
|
protocol_fail_count++;
|
2018-01-22 15:18:28 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
// wait for the expected number of reply bytes or timeout
|
|
|
|
if (!uart.wait_timeout(count*2+4, 10)) {
|
2024-05-30 12:31:36 -03:00
|
|
|
debug("t=%lu timeout read page=%u offset=%u count=%u avail=%u\n",
|
|
|
|
AP_HAL::millis(), page, offset, count, uart.available());
|
2020-01-17 02:36:01 -04:00
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
uint8_t *b = (uint8_t *)&pkt;
|
2017-12-26 19:03:21 -04:00
|
|
|
uint8_t n = uart.available();
|
2019-08-13 04:59:18 -03:00
|
|
|
if (n < offsetof(struct IOPacket, regs)) {
|
2020-07-11 09:22:05 -03:00
|
|
|
debug("t=%lu small pkt %u\n", AP_HAL::millis(), n);
|
2019-08-13 04:59:18 -03:00
|
|
|
protocol_fail_count++;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (pkt.get_size() != n) {
|
2020-07-11 09:22:05 -03:00
|
|
|
debug("t=%lu bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size());
|
2019-08-13 04:59:18 -03:00
|
|
|
protocol_fail_count++;
|
|
|
|
return false;
|
|
|
|
}
|
2023-02-21 19:27:43 -04:00
|
|
|
uart.read(b, MIN(n, sizeof(pkt)));
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
uint8_t got_crc = pkt.crc;
|
|
|
|
pkt.crc = 0;
|
|
|
|
uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
|
|
|
|
if (got_crc != expected_crc) {
|
2020-07-11 09:22:05 -03:00
|
|
|
debug("t=%lu bad crc %02x should be %02x n=%u %u/%u/%u\n",
|
2019-08-13 04:59:18 -03:00
|
|
|
AP_HAL::millis(), got_crc, expected_crc,
|
2018-10-30 21:07:47 -03:00
|
|
|
n, page, offset, count);
|
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (pkt.code != CODE_SUCCESS) {
|
2018-10-30 21:07:47 -03:00
|
|
|
debug("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count);
|
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (pkt.count < count) {
|
2018-10-30 21:07:47 -03:00
|
|
|
debug("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n);
|
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
memcpy(regs, pkt.regs, count*2);
|
2019-04-23 22:34:20 -03:00
|
|
|
if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
|
2019-04-21 23:54:00 -03:00
|
|
|
handle_repeated_failures();
|
|
|
|
}
|
2019-08-13 21:07:48 -03:00
|
|
|
total_errors += protocol_fail_count;
|
2018-10-30 21:07:47 -03:00
|
|
|
protocol_fail_count = 0;
|
2019-04-23 20:16:09 -03:00
|
|
|
protocol_count++;
|
2023-11-22 05:23:21 -04:00
|
|
|
last_reg_access_ms = AP_HAL::millis();
|
2017-12-27 03:24:15 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write count 16 bit registers
|
|
|
|
*/
|
|
|
|
bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
|
|
|
|
{
|
2023-12-21 11:06:36 -04:00
|
|
|
// The use of offset is very, very evil - it can either be a command within the page
|
|
|
|
// or a genuine offset, offsets within PAGE_SETUP are assumed to be commands, otherwise to be an
|
|
|
|
// actual offset
|
|
|
|
while (page != PAGE_SETUP && count > PKT_MAX_REGS) {
|
2018-10-30 21:07:47 -03:00
|
|
|
if (!write_registers(page, offset, PKT_MAX_REGS, regs)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
offset += PKT_MAX_REGS;
|
|
|
|
count -= PKT_MAX_REGS;
|
|
|
|
regs += PKT_MAX_REGS;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
IOPacket pkt;
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
discard_input();
|
|
|
|
|
|
|
|
memset(&pkt.regs[0], 0, count*2);
|
|
|
|
|
|
|
|
pkt.code = CODE_WRITE;
|
|
|
|
pkt.count = count;
|
|
|
|
pkt.page = page;
|
|
|
|
pkt.offset = offset;
|
|
|
|
pkt.crc = 0;
|
|
|
|
memcpy(pkt.regs, regs, 2*count);
|
|
|
|
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
|
2019-08-13 21:07:48 -03:00
|
|
|
|
|
|
|
const uint8_t pkt_size = pkt.get_size();
|
|
|
|
size_t ret = write_wait((uint8_t *)&pkt, pkt_size);
|
|
|
|
|
|
|
|
if (ret != pkt_size) {
|
|
|
|
debug("write failed2 %u %u %u %u\n", pkt_size, page, offset, ret);
|
2018-10-30 21:07:47 -03:00
|
|
|
protocol_fail_count++;
|
2018-01-22 15:18:28 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
// wait for the expected number of reply bytes or timeout
|
|
|
|
if (!uart.wait_timeout(4, 10)) {
|
2019-08-13 21:07:48 -03:00
|
|
|
debug("no reply for %u/%u/%u\n", page, offset, count);
|
2018-10-30 21:07:47 -03:00
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
uint8_t *b = (uint8_t *)&pkt;
|
|
|
|
uint8_t n = uart.available();
|
|
|
|
for (uint8_t i=0; i<n; i++) {
|
|
|
|
if (i < sizeof(pkt)) {
|
|
|
|
b[i] = uart.read();
|
2017-12-26 19:03:21 -04:00
|
|
|
}
|
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
|
|
|
|
if (pkt.code != CODE_SUCCESS) {
|
2018-10-30 21:07:47 -03:00
|
|
|
debug("bad code %02x write %u/%u/%u %02x/%02x n=%u\n",
|
|
|
|
pkt.code, page, offset, count,
|
|
|
|
pkt.page, pkt.offset, n);
|
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
uint8_t got_crc = pkt.crc;
|
|
|
|
pkt.crc = 0;
|
|
|
|
uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
|
|
|
|
if (got_crc != expected_crc) {
|
2018-10-30 21:07:47 -03:00
|
|
|
debug("bad crc %02x should be %02x\n", got_crc, expected_crc);
|
|
|
|
protocol_fail_count++;
|
2017-12-27 03:24:15 -04:00
|
|
|
return false;
|
|
|
|
}
|
2019-04-23 22:34:20 -03:00
|
|
|
if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
|
2019-04-21 23:54:00 -03:00
|
|
|
handle_repeated_failures();
|
|
|
|
}
|
2019-08-13 21:07:48 -03:00
|
|
|
total_errors += protocol_fail_count;
|
2018-10-30 21:07:47 -03:00
|
|
|
protocol_fail_count = 0;
|
2019-04-23 20:16:09 -03:00
|
|
|
protocol_count++;
|
2023-11-22 05:23:21 -04:00
|
|
|
|
|
|
|
last_reg_access_ms = AP_HAL::millis();
|
|
|
|
|
2017-12-27 03:24:15 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// modify a single register
|
|
|
|
bool AP_IOMCU::modify_register(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
|
|
|
|
{
|
|
|
|
uint16_t v = 0;
|
|
|
|
if (!read_registers(page, offset, 1, &v)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
uint16_t v2 = (v & ~clearbits) | setbits;
|
|
|
|
if (v2 == v) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return write_registers(page, offset, 1, &v2);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU::write_channel(uint8_t chan, uint16_t pwm)
|
|
|
|
{
|
2024-04-27 04:44:31 -03:00
|
|
|
if (chan >= IOMCU_MAX_RC_CHANNELS) { // could be SBUS out
|
2017-12-27 03:24:15 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (chan >= pwm_out.num_channels) {
|
|
|
|
pwm_out.num_channels = chan+1;
|
|
|
|
}
|
|
|
|
pwm_out.pwm[chan] = pwm;
|
|
|
|
if (!corked) {
|
|
|
|
push();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// trigger an ioevent
|
|
|
|
void AP_IOMCU::trigger_event(uint8_t event)
|
|
|
|
{
|
2018-01-12 22:33:28 -04:00
|
|
|
if (thread_ctx != nullptr) {
|
|
|
|
chEvtSignal(thread_ctx, EVENT_MASK(event));
|
2018-11-03 03:27:10 -03:00
|
|
|
} else {
|
|
|
|
// thread isn't started yet, trigger this event once it is started
|
|
|
|
initial_event_mask |= EVENT_MASK(event);
|
2018-01-12 22:33:28 -04:00
|
|
|
}
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// get state of safety switch
|
|
|
|
AP_HAL::Util::safety_state AP_IOMCU::get_safety_switch_state(void) const
|
|
|
|
{
|
|
|
|
return reg_status.flag_safety_off?AP_HAL::Util::SAFETY_ARMED:AP_HAL::Util::SAFETY_DISARMED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// force safety on
|
|
|
|
bool AP_IOMCU::force_safety_on(void)
|
|
|
|
{
|
|
|
|
trigger_event(IOEVENT_FORCE_SAFETY_ON);
|
2018-09-03 23:16:19 -03:00
|
|
|
safety_forced_off = false;
|
2017-12-27 03:24:15 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// force safety off
|
|
|
|
void AP_IOMCU::force_safety_off(void)
|
|
|
|
{
|
|
|
|
trigger_event(IOEVENT_FORCE_SAFETY_OFF);
|
2018-09-03 23:16:19 -03:00
|
|
|
safety_forced_off = true;
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// read from one channel
|
|
|
|
uint16_t AP_IOMCU::read_channel(uint8_t chan)
|
|
|
|
{
|
|
|
|
return pwm_in.pwm[chan];
|
|
|
|
}
|
|
|
|
|
|
|
|
// cork output
|
|
|
|
void AP_IOMCU::cork(void)
|
|
|
|
{
|
|
|
|
corked = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// push output
|
|
|
|
void AP_IOMCU::push(void)
|
|
|
|
{
|
|
|
|
trigger_event(IOEVENT_SEND_PWM_OUT);
|
|
|
|
corked = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// set output frequency
|
|
|
|
void AP_IOMCU::set_freq(uint16_t chmask, uint16_t freq)
|
|
|
|
{
|
2018-07-28 00:44:49 -03:00
|
|
|
// ensure mask is legal for the timer layout
|
2019-09-22 19:52:31 -03:00
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(ch_masks); i++) {
|
|
|
|
if (chmask & ch_masks[i]) {
|
|
|
|
chmask |= ch_masks[i];
|
2018-07-28 00:44:49 -03:00
|
|
|
}
|
|
|
|
}
|
2017-12-27 05:15:55 -04:00
|
|
|
rate.freq = freq;
|
2018-07-28 00:44:49 -03:00
|
|
|
rate.chmask |= chmask;
|
2017-12-27 05:15:55 -04:00
|
|
|
trigger_event(IOEVENT_SET_RATES);
|
2017-12-27 03:24:15 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// get output frequency
|
|
|
|
uint16_t AP_IOMCU::get_freq(uint16_t chan)
|
|
|
|
{
|
2017-12-27 05:15:55 -04:00
|
|
|
if ((1U<<chan) & rate.chmask) {
|
|
|
|
return rate.freq;
|
|
|
|
}
|
|
|
|
return rate.default_freq;
|
2017-12-26 19:03:21 -04:00
|
|
|
}
|
2017-12-31 22:28:59 -04:00
|
|
|
|
|
|
|
// enable SBUS out
|
|
|
|
bool AP_IOMCU::enable_sbus_out(uint16_t rate_hz)
|
|
|
|
{
|
|
|
|
rate.sbus_rate_hz = rate_hz;
|
|
|
|
trigger_event(IOEVENT_ENABLE_SBUS);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
check for new RC input
|
|
|
|
*/
|
|
|
|
bool AP_IOMCU::check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_chan)
|
|
|
|
{
|
2019-08-13 21:07:48 -03:00
|
|
|
if (last_frame_us != uint32_t(rc_last_input_ms * 1000U)) {
|
2023-11-13 15:58:36 -04:00
|
|
|
num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_RC_CHANNELS), max_chan);
|
2017-12-31 22:28:59 -04:00
|
|
|
memcpy(channels, rc_input.pwm, num_channels*2);
|
2019-08-13 21:07:48 -03:00
|
|
|
last_frame_us = uint32_t(rc_last_input_ms * 1000U);
|
2017-12-31 22:28:59 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2018-01-02 01:47:42 -04:00
|
|
|
|
2018-01-03 02:25:30 -04:00
|
|
|
// set IMU heater target
|
|
|
|
void AP_IOMCU::set_heater_duty_cycle(uint8_t duty_cycle)
|
|
|
|
{
|
|
|
|
heater_duty_cycle = duty_cycle;
|
|
|
|
trigger_event(IOEVENT_SET_HEATER_TARGET);
|
|
|
|
}
|
|
|
|
|
2018-01-12 23:52:29 -04:00
|
|
|
// set default output rate
|
|
|
|
void AP_IOMCU::set_default_rate(uint16_t rate_hz)
|
|
|
|
{
|
2018-01-22 15:18:28 -04:00
|
|
|
if (rate.default_freq != rate_hz) {
|
|
|
|
rate.default_freq = rate_hz;
|
|
|
|
trigger_event(IOEVENT_SET_DEFAULT_RATE);
|
|
|
|
}
|
2018-01-12 23:52:29 -04:00
|
|
|
}
|
|
|
|
|
2018-01-16 01:37:14 -04:00
|
|
|
// setup for oneshot mode
|
|
|
|
void AP_IOMCU::set_oneshot_mode(void)
|
|
|
|
{
|
|
|
|
trigger_event(IOEVENT_SET_ONESHOT_ON);
|
2021-02-23 17:47:24 -04:00
|
|
|
rate.oneshot_enabled = true;
|
2018-01-16 01:37:14 -04:00
|
|
|
}
|
|
|
|
|
2018-07-12 23:28:43 -03:00
|
|
|
// setup for brushed mode
|
|
|
|
void AP_IOMCU::set_brushed_mode(void)
|
|
|
|
{
|
|
|
|
trigger_event(IOEVENT_SET_BRUSHED_ON);
|
2021-02-23 17:47:24 -04:00
|
|
|
rate.brushed_enabled = true;
|
2018-07-12 23:28:43 -03:00
|
|
|
}
|
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
#if HAL_DSHOT_ENABLED
|
2023-08-14 04:43:33 -03:00
|
|
|
// directly set the dshot rate - period_us is the dshot tick period_us and drate is the number
|
|
|
|
// of dshot ticks per main loop cycle. These values are calculated by RCOutput::set_dshot_rate()
|
|
|
|
// if the backend is free running then then period_us is fixed at 1000us and drate is 0
|
2023-05-24 17:43:17 -03:00
|
|
|
void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate)
|
|
|
|
{
|
|
|
|
dshot_rate.period_us = period_us;
|
|
|
|
dshot_rate.rate = drate;
|
|
|
|
trigger_event(IOEVENT_SET_DSHOT_PERIOD);
|
|
|
|
}
|
|
|
|
|
2023-06-23 17:15:14 -03:00
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
// set output mode
|
|
|
|
void AP_IOMCU::set_telem_request_mask(uint32_t mask)
|
|
|
|
{
|
2023-07-10 06:32:29 -03:00
|
|
|
page_dshot dshot {
|
|
|
|
.telem_mask = uint16_t(mask)
|
|
|
|
};
|
|
|
|
dshot_command_queue.push(dshot);
|
2023-05-24 17:43:17 -03:00
|
|
|
trigger_event(IOEVENT_DSHOT);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
|
|
|
|
{
|
2023-07-10 06:32:29 -03:00
|
|
|
page_dshot dshot {
|
|
|
|
.command = command,
|
|
|
|
.chan = chan,
|
|
|
|
.command_timeout_ms = command_timeout_ms,
|
|
|
|
.repeat_count = uint8_t(repeat_count),
|
|
|
|
.priority = priority
|
|
|
|
};
|
|
|
|
dshot_command_queue.push(dshot);
|
2023-05-24 17:43:17 -03:00
|
|
|
trigger_event(IOEVENT_DSHOT);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2020-07-11 09:22:05 -03:00
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
2023-06-23 17:15:14 -03:00
|
|
|
// set output mode
|
|
|
|
void AP_IOMCU::set_bidir_dshot_mask(uint16_t mask)
|
|
|
|
{
|
|
|
|
mode_out.bdmask = mask;
|
|
|
|
trigger_event(IOEVENT_SET_OUTPUT_MODE);
|
|
|
|
}
|
|
|
|
|
2024-08-13 09:59:08 -03:00
|
|
|
// set reversible mask
|
|
|
|
void AP_IOMCU::set_reversible_mask(uint16_t mask)
|
|
|
|
{
|
|
|
|
mode_out.reversible_mask = mask;
|
|
|
|
trigger_event(IOEVENT_SET_OUTPUT_MODE);
|
|
|
|
}
|
|
|
|
|
2023-06-17 19:26:56 -03:00
|
|
|
AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const
|
|
|
|
{
|
|
|
|
mask = reg_status.rcout_mask;
|
|
|
|
return AP_HAL::RCOutput::output_mode(reg_status.rcout_mode);
|
|
|
|
}
|
|
|
|
|
2024-08-30 10:56:12 -03:00
|
|
|
uint32_t AP_IOMCU::get_disabled_channels(uint32_t digital_mask) const
|
|
|
|
{
|
|
|
|
uint32_t dig_out = reg_status.rcout_mask & (digital_mask & 0xFF);
|
|
|
|
if (dig_out > 0
|
|
|
|
&& AP_HAL::RCOutput::is_dshot_protocol(AP_HAL::RCOutput::output_mode(reg_status.rcout_mode))) {
|
|
|
|
return ~dig_out & 0xFF;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
// setup channels
|
|
|
|
void AP_IOMCU::enable_ch(uint8_t ch)
|
|
|
|
{
|
2023-08-14 04:43:33 -03:00
|
|
|
if (!(pwm_out.channel_mask & (1U << ch))) {
|
|
|
|
pwm_out.channel_mask |= (1U << ch);
|
2023-05-24 17:43:17 -03:00
|
|
|
trigger_event(IOEVENT_SET_CHANNEL_MASK);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU::disable_ch(uint8_t ch)
|
|
|
|
{
|
2023-08-14 04:43:33 -03:00
|
|
|
if (pwm_out.channel_mask & (1U << ch)) {
|
2023-05-24 17:43:17 -03:00
|
|
|
pwm_out.channel_mask &= ~(1U << ch);
|
|
|
|
trigger_event(IOEVENT_SET_CHANNEL_MASK);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-13 03:17:08 -03:00
|
|
|
// handling of BRD_SAFETYOPTION parameter
|
|
|
|
void AP_IOMCU::update_safety_options(void)
|
|
|
|
{
|
2019-02-10 14:06:22 -04:00
|
|
|
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
|
2018-04-13 03:17:08 -03:00
|
|
|
if (!boardconfig) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
uint16_t desired_options = 0;
|
|
|
|
uint16_t options = boardconfig->get_safety_button_options();
|
2024-08-13 09:59:08 -03:00
|
|
|
bool armed = hal.util->get_soft_armed();
|
2018-04-13 03:17:08 -03:00
|
|
|
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
|
|
|
|
desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_OFF;
|
|
|
|
}
|
|
|
|
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
|
|
|
|
desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_ON;
|
|
|
|
}
|
2024-08-13 09:59:08 -03:00
|
|
|
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && armed) {
|
2018-04-13 03:17:08 -03:00
|
|
|
desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
|
|
|
|
}
|
2024-08-13 09:59:08 -03:00
|
|
|
// update armed state
|
|
|
|
if (armed) {
|
|
|
|
desired_options |= P_SETUP_ARMING_FMU_ARMED;
|
|
|
|
}
|
|
|
|
|
2018-04-13 03:17:08 -03:00
|
|
|
if (last_safety_options != desired_options) {
|
2024-08-13 09:59:08 -03:00
|
|
|
uint16_t mask = (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF | P_SETUP_ARMING_FMU_ARMED);
|
2018-04-13 18:00:26 -03:00
|
|
|
uint32_t bits_to_set = desired_options & mask;
|
|
|
|
uint32_t bits_to_clear = (~desired_options) & mask;
|
2018-04-13 03:17:08 -03:00
|
|
|
if (modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, bits_to_clear, bits_to_set)) {
|
|
|
|
last_safety_options = desired_options;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-08-12 23:28:47 -03:00
|
|
|
// update enabled RC protocols mask
|
|
|
|
void AP_IOMCU::send_rc_protocols()
|
|
|
|
{
|
|
|
|
const uint32_t v = rc().enabled_protocols();
|
|
|
|
if (last_rc_protocols == v) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (write_registers(PAGE_SETUP, PAGE_REG_SETUP_RC_PROTOCOLS, 2, (uint16_t *)&v)) {
|
|
|
|
last_rc_protocols = v;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-14 08:09:11 -03:00
|
|
|
/*
|
|
|
|
check ROMFS firmware against CRC on IOMCU, and if incorrect then upload new firmware
|
|
|
|
*/
|
|
|
|
bool AP_IOMCU::check_crc(void)
|
|
|
|
{
|
|
|
|
// flash size minus 4k bootloader
|
|
|
|
const uint32_t flash_size = 0x10000 - 0x1000;
|
2023-08-14 04:43:33 -03:00
|
|
|
const char *path = AP_BoardConfig::io_dshot() ? dshot_fw_name : fw_name;
|
|
|
|
|
|
|
|
fw = AP_ROMFS::find_decompress(path, fw_size);
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2018-04-14 08:09:11 -03:00
|
|
|
if (!fw) {
|
2023-08-14 04:43:33 -03:00
|
|
|
DEV_PRINTF("failed to find %s\n", path);
|
2018-04-14 08:09:11 -03:00
|
|
|
return false;
|
|
|
|
}
|
2019-10-27 17:35:54 -03:00
|
|
|
uint32_t crc = crc32_small(0, fw, fw_size);
|
2018-04-14 08:09:11 -03:00
|
|
|
|
2018-07-10 20:51:43 -03:00
|
|
|
// pad CRC to max size
|
|
|
|
for (uint32_t i=0; i<flash_size-fw_size; i++) {
|
2018-04-14 08:09:11 -03:00
|
|
|
uint8_t b = 0xff;
|
2019-10-27 17:35:54 -03:00
|
|
|
crc = crc32_small(crc, &b, 1);
|
2018-04-14 08:09:11 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t io_crc = 0;
|
2018-10-29 18:50:59 -03:00
|
|
|
uint8_t tries = 32;
|
|
|
|
while (tries--) {
|
|
|
|
if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc)) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (io_crc == crc) {
|
2022-03-21 06:35:24 -03:00
|
|
|
DEV_PRINTF("IOMCU: CRC ok\n");
|
2018-04-16 19:04:33 -03:00
|
|
|
crc_is_ok = true;
|
2019-10-23 06:58:01 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2018-07-09 03:47:35 -03:00
|
|
|
fw = nullptr;
|
2018-04-14 08:09:11 -03:00
|
|
|
return true;
|
2018-05-05 15:16:52 -03:00
|
|
|
} else {
|
2022-03-21 06:35:24 -03:00
|
|
|
DEV_PRINTF("IOMCU: CRC mismatch expected: 0x%X got: 0x%X\n", (unsigned)crc, (unsigned)io_crc);
|
2018-04-14 08:09:11 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
const uint16_t magic = REBOOT_BL_MAGIC;
|
|
|
|
write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
|
|
|
|
|
2023-02-10 21:40:26 -04:00
|
|
|
// avoid internal error on fw upload delay
|
2023-11-22 05:23:21 -04:00
|
|
|
last_reg_access_ms = 0;
|
2023-02-10 21:40:26 -04:00
|
|
|
|
2018-07-09 03:47:35 -03:00
|
|
|
if (!upload_fw()) {
|
2019-10-23 06:58:01 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2018-07-09 03:47:35 -03:00
|
|
|
fw = nullptr;
|
2019-11-06 15:44:58 -04:00
|
|
|
AP_BoardConfig::config_error("Failed to update IO firmware");
|
2018-05-15 23:53:23 -03:00
|
|
|
}
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2019-10-23 06:58:01 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2018-07-09 03:47:35 -03:00
|
|
|
fw = nullptr;
|
2018-04-14 08:09:11 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2018-09-12 17:22:26 -03:00
|
|
|
/*
|
|
|
|
set the pwm to use when in FMU failsafe
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::set_failsafe_pwm(uint16_t chmask, uint16_t period_us)
|
|
|
|
{
|
|
|
|
bool changed = false;
|
2024-04-27 04:44:31 -03:00
|
|
|
for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
|
2018-09-12 17:22:26 -03:00
|
|
|
if (chmask & (1U<<i)) {
|
|
|
|
if (pwm_out.failsafe_pwm[i] != period_us) {
|
|
|
|
pwm_out.failsafe_pwm[i] = period_us;
|
|
|
|
changed = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (changed) {
|
|
|
|
pwm_out.failsafe_pwm_set++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-14 00:54:04 -03:00
|
|
|
|
|
|
|
// set mask of channels that ignore safety state
|
|
|
|
void AP_IOMCU::set_safety_mask(uint16_t chmask)
|
|
|
|
{
|
|
|
|
if (pwm_out.safety_mask != chmask) {
|
|
|
|
pwm_out.safety_mask = chmask;
|
2019-10-20 10:47:14 -03:00
|
|
|
trigger_event(IOEVENT_SET_SAFETY_MASK);
|
2018-04-14 00:54:04 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-16 19:04:33 -03:00
|
|
|
/*
|
|
|
|
check that IO is healthy. This should be used in arming checks
|
|
|
|
*/
|
|
|
|
bool AP_IOMCU::healthy(void)
|
|
|
|
{
|
2020-01-17 02:36:01 -04:00
|
|
|
return crc_is_ok && protocol_fail_count == 0 && !detected_io_reset && read_status_errors < read_status_ok/128U;
|
2018-04-16 19:04:33 -03:00
|
|
|
}
|
|
|
|
|
2018-10-29 18:50:59 -03:00
|
|
|
/*
|
|
|
|
shutdown protocol, ready for reboot
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::shutdown(void)
|
|
|
|
{
|
|
|
|
do_shutdown = true;
|
|
|
|
while (!done_shutdown) {
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
/*
|
|
|
|
reboot IOMCU
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::soft_reboot(void)
|
|
|
|
{
|
|
|
|
const uint16_t magic = REBOOT_BL_MAGIC;
|
|
|
|
write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2018-08-26 12:06:10 -03:00
|
|
|
/*
|
|
|
|
request bind on a DSM radio
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::bind_dsm(uint8_t mode)
|
|
|
|
{
|
2023-02-10 19:44:09 -04:00
|
|
|
if (!is_chibios_backend || AP::arming().is_armed()) {
|
2018-08-26 12:06:10 -03:00
|
|
|
// only with ChibiOS IO firmware, and disarmed
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
uint16_t reg = mode;
|
2018-10-31 19:18:13 -03:00
|
|
|
write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSM_BIND, 1, ®);
|
2018-08-26 12:06:10 -03:00
|
|
|
}
|
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
/*
|
|
|
|
setup for mixing. This allows fixed wing aircraft to fly in manual
|
|
|
|
mode if the FMU dies
|
|
|
|
*/
|
2024-09-18 23:25:23 -03:00
|
|
|
bool AP_IOMCU::setup_mixing(int8_t override_chan,
|
2018-10-31 01:09:49 -03:00
|
|
|
float mixing_gain, uint16_t manual_rc_mask)
|
2018-10-30 21:07:47 -03:00
|
|
|
{
|
2018-11-01 03:39:24 -03:00
|
|
|
if (!is_chibios_backend) {
|
2018-10-30 21:07:47 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
bool changed = false;
|
|
|
|
#define MIX_UPDATE(a,b) do { if ((a) != (b)) { a = b; changed = true; }} while (0)
|
|
|
|
|
|
|
|
// update mixing structure, checking for changes
|
2024-04-27 04:44:31 -03:00
|
|
|
for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
|
2019-02-05 22:22:55 -04:00
|
|
|
const SRV_Channel *c = SRV_Channels::srv_channel(i);
|
|
|
|
if (!c) {
|
2018-10-30 21:07:47 -03:00
|
|
|
continue;
|
|
|
|
}
|
2019-02-05 22:22:55 -04:00
|
|
|
MIX_UPDATE(mixing.servo_trim[i], c->get_trim());
|
|
|
|
MIX_UPDATE(mixing.servo_min[i], c->get_output_min());
|
|
|
|
MIX_UPDATE(mixing.servo_max[i], c->get_output_max());
|
|
|
|
MIX_UPDATE(mixing.servo_function[i], c->get_function());
|
|
|
|
MIX_UPDATE(mixing.servo_reversed[i], c->get_reversed());
|
2018-10-30 21:07:47 -03:00
|
|
|
}
|
2024-09-18 23:25:23 -03:00
|
|
|
auto &xrc = rc();
|
|
|
|
// note that if not all of these channels are specified correctly
|
|
|
|
// in parameters then these may be a "dummy" RC channel pointer.
|
|
|
|
// In that case c->ch() will be zero.
|
|
|
|
const RC_Channel *channels[] {
|
|
|
|
&xrc.get_roll_channel(),
|
|
|
|
&xrc.get_pitch_channel(),
|
|
|
|
&xrc.get_throttle_channel(),
|
|
|
|
&xrc.get_yaw_channel()
|
|
|
|
};
|
2018-10-30 21:07:47 -03:00
|
|
|
for (uint8_t i=0; i<4; i++) {
|
2024-09-18 23:25:23 -03:00
|
|
|
const auto *c = channels[i];
|
|
|
|
MIX_UPDATE(mixing.rc_channel[i], c->ch());
|
2019-02-05 22:22:55 -04:00
|
|
|
MIX_UPDATE(mixing.rc_min[i], c->get_radio_min());
|
|
|
|
MIX_UPDATE(mixing.rc_max[i], c->get_radio_max());
|
|
|
|
MIX_UPDATE(mixing.rc_trim[i], c->get_radio_trim());
|
|
|
|
MIX_UPDATE(mixing.rc_reversed[i], c->get_reverse());
|
2018-10-30 23:10:51 -03:00
|
|
|
|
|
|
|
// cope with reversible throttle
|
2022-02-25 02:01:06 -04:00
|
|
|
if (i == 2 && c->get_type() == RC_Channel::ControlType::ANGLE) {
|
2018-10-30 23:10:51 -03:00
|
|
|
MIX_UPDATE(mixing.throttle_is_angle, 1);
|
|
|
|
} else {
|
|
|
|
MIX_UPDATE(mixing.throttle_is_angle, 0);
|
|
|
|
}
|
2018-10-30 21:07:47 -03:00
|
|
|
}
|
2018-10-30 23:10:51 -03:00
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
MIX_UPDATE(mixing.rc_chan_override, override_chan);
|
2018-10-31 00:16:17 -03:00
|
|
|
MIX_UPDATE(mixing.mixing_gain, (uint16_t)(mixing_gain*1000));
|
2018-10-31 01:09:49 -03:00
|
|
|
MIX_UPDATE(mixing.manual_rc_mask, manual_rc_mask);
|
2018-10-30 23:10:51 -03:00
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
// and enable
|
|
|
|
MIX_UPDATE(mixing.enabled, 1);
|
|
|
|
if (changed) {
|
|
|
|
trigger_event(IOEVENT_MIXING);
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-11-05 01:44:20 -04:00
|
|
|
/*
|
|
|
|
return the RC protocol name
|
|
|
|
*/
|
|
|
|
const char *AP_IOMCU::get_rc_protocol(void)
|
|
|
|
{
|
|
|
|
if (!is_chibios_backend) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2019-08-13 21:07:48 -03:00
|
|
|
return AP_RCProtocol::protocol_name_from_protocol((AP_RCProtocol::rcprotocol_t)rc_input.rc_protocol);
|
2018-11-05 01:44:20 -04:00
|
|
|
}
|
|
|
|
|
2019-04-21 23:54:00 -03:00
|
|
|
/*
|
|
|
|
we have had a series of repeated protocol failures to the
|
|
|
|
IOMCU. This may indicate that the IOMCU has been reset (possibly due
|
|
|
|
to a watchdog).
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::handle_repeated_failures(void)
|
|
|
|
{
|
2019-04-23 20:16:09 -03:00
|
|
|
if (protocol_count < 100) {
|
|
|
|
// we're just starting up, ignore initial failures caused by
|
|
|
|
// initial sync with IOMCU
|
|
|
|
return;
|
|
|
|
}
|
2020-04-29 21:40:46 -03:00
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::iomcu_fail);
|
2019-04-23 22:34:20 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
check for IOMCU reset (possibly due to a watchdog).
|
|
|
|
*/
|
|
|
|
void AP_IOMCU::check_iomcu_reset(void)
|
|
|
|
{
|
|
|
|
if (last_iocmu_timestamp_ms == 0) {
|
|
|
|
// initialisation
|
|
|
|
last_iocmu_timestamp_ms = reg_status.timestamp_ms;
|
2022-03-21 06:35:24 -03:00
|
|
|
DEV_PRINTF("IOMCU startup\n");
|
2019-04-23 22:34:20 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
uint32_t dt_ms = reg_status.timestamp_ms - last_iocmu_timestamp_ms;
|
2023-02-10 19:44:09 -04:00
|
|
|
#if IOMCU_DEBUG_ENABLE
|
|
|
|
const uint32_t ts1 = last_iocmu_timestamp_ms;
|
|
|
|
#endif
|
2021-01-21 15:38:06 -04:00
|
|
|
// when we are in an expected delay allow for a larger time
|
|
|
|
// delta. This copes with flash erase, such as bootloader update
|
2023-06-01 12:06:52 -03:00
|
|
|
const uint32_t max_delay = hal.scheduler->in_expected_delay()?8000:500;
|
2019-04-23 22:34:20 -03:00
|
|
|
last_iocmu_timestamp_ms = reg_status.timestamp_ms;
|
2021-01-21 15:38:06 -04:00
|
|
|
|
|
|
|
if (dt_ms < max_delay) {
|
2019-04-23 22:34:20 -03:00
|
|
|
// all OK
|
2021-05-21 18:04:43 -03:00
|
|
|
last_safety_off = reg_status.flag_safety_off;
|
2019-04-23 22:34:20 -03:00
|
|
|
return;
|
|
|
|
}
|
2019-04-21 23:54:00 -03:00
|
|
|
detected_io_reset = true;
|
2020-04-29 21:40:46 -03:00
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
|
2023-02-10 19:44:09 -04:00
|
|
|
debug("IOMCU reset t=%u %u %u dt=%u\n",
|
|
|
|
unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms));
|
2020-05-12 07:26:37 -03:00
|
|
|
|
2023-02-10 19:44:09 -04:00
|
|
|
bool have_forced_off = false;
|
|
|
|
if (last_safety_off && !reg_status.flag_safety_off && AP::arming().is_armed()) {
|
2021-05-21 18:04:43 -03:00
|
|
|
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
|
|
|
|
uint16_t options = boardconfig?boardconfig->get_safety_button_options():0;
|
|
|
|
if (safety_forced_off || (options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) == 0) {
|
|
|
|
// IOMCU has reset while armed with safety off - force it off
|
|
|
|
// again so we can keep flying
|
2023-02-10 19:44:09 -04:00
|
|
|
have_forced_off = true;
|
2021-05-21 18:04:43 -03:00
|
|
|
force_safety_off();
|
|
|
|
}
|
2020-05-12 07:26:37 -03:00
|
|
|
}
|
2023-02-10 19:44:09 -04:00
|
|
|
if (!have_forced_off) {
|
|
|
|
last_safety_off = reg_status.flag_safety_off;
|
|
|
|
}
|
2020-05-12 07:26:37 -03:00
|
|
|
|
2019-04-21 23:54:00 -03:00
|
|
|
// we need to ensure the mixer data and the rates are sent over to
|
|
|
|
// the IOMCU
|
|
|
|
if (mixing.enabled) {
|
|
|
|
trigger_event(IOEVENT_MIXING);
|
|
|
|
}
|
|
|
|
trigger_event(IOEVENT_SET_RATES);
|
2019-04-23 22:34:20 -03:00
|
|
|
trigger_event(IOEVENT_SET_DEFAULT_RATE);
|
2023-05-24 17:43:17 -03:00
|
|
|
trigger_event(IOEVENT_SET_DSHOT_PERIOD);
|
|
|
|
trigger_event(IOEVENT_SET_OUTPUT_MODE);
|
|
|
|
trigger_event(IOEVENT_SET_CHANNEL_MASK);
|
2021-02-23 17:47:24 -04:00
|
|
|
if (rate.oneshot_enabled) {
|
|
|
|
trigger_event(IOEVENT_SET_ONESHOT_ON);
|
|
|
|
}
|
|
|
|
if (rate.brushed_enabled) {
|
|
|
|
trigger_event(IOEVENT_SET_BRUSHED_ON);
|
|
|
|
}
|
|
|
|
if (rate.sbus_rate_hz) {
|
|
|
|
trigger_event(IOEVENT_ENABLE_SBUS);
|
|
|
|
}
|
|
|
|
if (pwm_out.safety_mask) {
|
|
|
|
trigger_event(IOEVENT_SET_SAFETY_MASK);
|
|
|
|
}
|
2020-08-12 23:28:47 -03:00
|
|
|
last_rc_protocols = 0;
|
2019-04-21 23:54:00 -03:00
|
|
|
}
|
|
|
|
|
2022-04-20 22:14:10 -03:00
|
|
|
// Check if pin number is valid and configured for GPIO
|
2021-09-20 10:44:12 -03:00
|
|
|
bool AP_IOMCU::valid_GPIO_pin(uint8_t pin) const
|
|
|
|
{
|
2022-04-20 22:14:10 -03:00
|
|
|
// sanity check pin number
|
|
|
|
if (!convert_pin_number(pin)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check pin is enabled as GPIO
|
|
|
|
return ((GPIO.channel_mask & (1U << pin)) != 0);
|
2021-09-20 10:44:12 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// convert external pin numbers 101 to 108 to internal 0 to 7
|
|
|
|
bool AP_IOMCU::convert_pin_number(uint8_t& pin) const
|
|
|
|
{
|
2022-04-20 21:45:10 -03:00
|
|
|
if (pin < 101 || pin > 108) {
|
2021-09-20 10:44:12 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
pin -= 101;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// set GPIO mask of channels setup for output
|
|
|
|
void AP_IOMCU::set_GPIO_mask(uint8_t mask)
|
|
|
|
{
|
|
|
|
if (mask == GPIO.channel_mask) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
GPIO.channel_mask = mask;
|
|
|
|
trigger_event(IOEVENT_GPIO);
|
|
|
|
}
|
|
|
|
|
2024-07-28 13:32:50 -03:00
|
|
|
// Get GPIO mask of channels setup for output
|
|
|
|
uint8_t AP_IOMCU::get_GPIO_mask() const
|
|
|
|
{
|
|
|
|
return GPIO.channel_mask;
|
|
|
|
}
|
|
|
|
|
2021-09-20 10:44:12 -03:00
|
|
|
// write to a output pin
|
|
|
|
void AP_IOMCU::write_GPIO(uint8_t pin, bool value)
|
|
|
|
{
|
|
|
|
if (!convert_pin_number(pin)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (value == ((GPIO.output_mask & (1U << pin)) != 0)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (value) {
|
|
|
|
GPIO.output_mask |= (1U << pin);
|
|
|
|
} else {
|
|
|
|
GPIO.output_mask &= ~(1U << pin);
|
|
|
|
}
|
|
|
|
trigger_event(IOEVENT_GPIO);
|
|
|
|
}
|
|
|
|
|
2024-07-28 13:32:50 -03:00
|
|
|
// Read the last output value send to the GPIO pin
|
|
|
|
// This is not a real read of the actual pin
|
|
|
|
// This allows callers to check for state change
|
|
|
|
uint8_t AP_IOMCU::read_virtual_GPIO(uint8_t pin) const
|
|
|
|
{
|
|
|
|
if (!convert_pin_number(pin)) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
return (GPIO.output_mask & (1U << pin)) != 0;
|
|
|
|
}
|
|
|
|
|
2021-09-20 10:44:12 -03:00
|
|
|
// toggle a output pin
|
|
|
|
void AP_IOMCU::toggle_GPIO(uint8_t pin)
|
|
|
|
{
|
|
|
|
if (!convert_pin_number(pin)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
GPIO.output_mask ^= (1U << pin);
|
|
|
|
trigger_event(IOEVENT_GPIO);
|
|
|
|
}
|
|
|
|
|
2019-11-01 23:32:48 -03:00
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
AP_IOMCU *iomcu(void) {
|
|
|
|
return AP_IOMCU::get_singleton();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
2018-01-02 01:47:42 -04:00
|
|
|
#endif // HAL_WITH_IO_MCU
|