mirror of https://github.com/ArduPilot/ardupilot
AP_IOMCU: constrain PWM channels to 8, telem channels to 4 and RC channels to 16
make ADC readings interrupt driven turn off iomcu updates when debugging allow for correct number of telemetry channels cycle between vservo and vrssi when reading adc build adc with O2
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
8c03c9e4bf
commit
43b6fc0dba
|
|
@ -78,12 +78,16 @@ void AP_IOMCU::init(void)
|
|||
uart.begin(1500*1000, 128, 128);
|
||||
uart.set_unbuffered_writes(true);
|
||||
|
||||
#if IOMCU_DEBUG_ENABLE
|
||||
crc_is_ok = true;
|
||||
#else
|
||||
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
|
||||
if ((!boardconfig || boardconfig->io_enabled() == 1) && !hal.util->was_watchdog_reset()) {
|
||||
check_crc();
|
||||
} else {
|
||||
crc_is_ok = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
|
||||
1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
|
||||
|
|
@ -407,7 +411,7 @@ void AP_IOMCU::read_erpm()
|
|||
if (blh) {
|
||||
motor_poles = blh->get_motor_poles();
|
||||
}
|
||||
for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS/4; i++) {
|
||||
for (uint8_t i = 0; i < IOMCU_MAX_TELEM_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) {
|
||||
|
|
@ -426,15 +430,11 @@ void AP_IOMCU::read_telem()
|
|||
uint16_t *r = (uint16_t *)telem;
|
||||
iopage page = PAGE_RAW_DSHOT_TELEM_1_4;
|
||||
switch (esc_group) {
|
||||
#if IOMCU_MAX_TELEM_CHANNELS > 4
|
||||
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;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -450,7 +450,7 @@ void AP_IOMCU::read_telem()
|
|||
};
|
||||
update_telem_data(esc_group * 4 + i, t, telem->types[i]);
|
||||
}
|
||||
esc_group = (esc_group + 1) % 4;
|
||||
esc_group = (esc_group + 1) % (IOMCU_MAX_TELEM_CHANNELS / 4);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -874,7 +874,7 @@ bool AP_IOMCU::enable_sbus_out(uint16_t rate_hz)
|
|||
bool AP_IOMCU::check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_chan)
|
||||
{
|
||||
if (last_frame_us != uint32_t(rc_last_input_ms * 1000U)) {
|
||||
num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_CHANNELS), max_chan);
|
||||
num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_RC_CHANNELS), max_chan);
|
||||
memcpy(channels, rc_input.pwm, num_channels*2);
|
||||
last_frame_us = uint32_t(rc_last_input_ms * 1000U);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -273,7 +273,7 @@ private:
|
|||
|
||||
// bi-directional dshot erpm values
|
||||
struct page_dshot_erpm dshot_erpm;
|
||||
struct page_dshot_telem dshot_telem[IOMCU_MAX_CHANNELS/4];
|
||||
struct page_dshot_telem dshot_telem[IOMCU_MAX_TELEM_CHANNELS/4];
|
||||
uint8_t esc_group;
|
||||
|
||||
// queue of dshot commands that need sending
|
||||
|
|
|
|||
|
|
@ -468,7 +468,7 @@ void AP_IOMCU_FW::update()
|
|||
now - sbus_last_ms >= sbus_interval_ms) {
|
||||
// output a new SBUS frame
|
||||
sbus_last_ms = now;
|
||||
sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
|
||||
sbus_out_write(reg_servo.pwm, IOMCU_MAX_RC_CHANNELS);
|
||||
}
|
||||
// handle FMU failsafe
|
||||
if (now - fmu_data_received_time > 200) {
|
||||
|
|
@ -583,7 +583,7 @@ void AP_IOMCU_FW::rcin_update()
|
|||
const auto &rc = AP::RC();
|
||||
rc_input.count = hal.rcin->num_channels();
|
||||
rc_input.flags_rc_ok = true;
|
||||
hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS);
|
||||
hal.rcin->read(rc_input.pwm, IOMCU_MAX_RC_CHANNELS);
|
||||
rc_last_input_ms = last_ms;
|
||||
rc_input.rc_protocol = (uint16_t)rc.protocol_detected();
|
||||
rc_input.rssi = rc.get_RSSI();
|
||||
|
|
@ -606,7 +606,7 @@ void AP_IOMCU_FW::rcin_update()
|
|||
if (mixing.enabled &&
|
||||
mixing.rc_chan_override > 0 &&
|
||||
rc_input.flags_rc_ok &&
|
||||
mixing.rc_chan_override <= IOMCU_MAX_CHANNELS) {
|
||||
mixing.rc_chan_override <= IOMCU_MAX_RC_CHANNELS) {
|
||||
override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1750);
|
||||
} else {
|
||||
override_active = false;
|
||||
|
|
@ -625,7 +625,7 @@ void AP_IOMCU_FW::erpm_update()
|
|||
uint32_t now_us = AP_HAL::micros();
|
||||
|
||||
if (hal.rcout->new_erpm()) {
|
||||
dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_CHANNELS);
|
||||
dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_TELEM_CHANNELS);
|
||||
last_erpm_us = now_us;
|
||||
} else if (now_us - last_erpm_us > ESC_RPM_DATA_TIMEOUT_US) {
|
||||
dshot_erpm.update_mask = 0;
|
||||
|
|
@ -636,10 +636,10 @@ 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 i = 0; i < IOMCU_MAX_TELEM_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) {
|
||||
if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) {
|
||||
continue;
|
||||
}
|
||||
dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
|
||||
|
|
@ -767,15 +767,11 @@ bool AP_IOMCU_FW::handle_code_read()
|
|||
case PAGE_RAW_DSHOT_TELEM_1_4:
|
||||
COPY_PAGE(dshot_telem[0]);
|
||||
break;
|
||||
#if IOMCU_MAX_TELEM_CHANNELS > 4
|
||||
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
|
||||
#endif
|
||||
case PAGE_STATUS:
|
||||
COPY_PAGE(reg_status);
|
||||
|
|
|
|||
|
|
@ -141,7 +141,7 @@ public:
|
|||
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||
struct page_dshot_erpm dshot_erpm;
|
||||
uint32_t last_erpm_us;
|
||||
struct page_dshot_telem dshot_telem[IOMCU_MAX_CHANNELS/4];
|
||||
struct page_dshot_telem dshot_telem[IOMCU_MAX_TELEM_CHANNELS/4];
|
||||
uint32_t last_telem_ms;
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
AP_ESC_Telem esc_telem;
|
||||
|
|
|
|||
|
|
@ -9,7 +9,9 @@
|
|||
|
||||
// 22 is enough for the rc_input page in one transfer
|
||||
#define PKT_MAX_REGS 22
|
||||
#define IOMCU_MAX_CHANNELS 16
|
||||
#define IOMCU_MAX_RC_CHANNELS 16
|
||||
#define IOMCU_MAX_CHANNELS 8
|
||||
#define IOMCU_MAX_TELEM_CHANNELS 4
|
||||
|
||||
//#define IOMCU_DEBUG
|
||||
|
||||
|
|
@ -136,6 +138,7 @@ struct page_reg_status {
|
|||
uint8_t err_write;
|
||||
uint8_t err_uart;
|
||||
uint8_t err_lock;
|
||||
uint8_t spare;
|
||||
};
|
||||
|
||||
struct page_rc_input {
|
||||
|
|
@ -143,7 +146,7 @@ struct page_rc_input {
|
|||
uint8_t flags_failsafe:1;
|
||||
uint8_t flags_rc_ok:1;
|
||||
uint8_t rc_protocol;
|
||||
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
||||
uint16_t pwm[IOMCU_MAX_RC_CHANNELS];
|
||||
int16_t rssi;
|
||||
};
|
||||
|
||||
|
|
@ -206,7 +209,7 @@ struct page_dshot {
|
|||
};
|
||||
|
||||
struct page_dshot_erpm {
|
||||
uint16_t erpm[IOMCU_MAX_CHANNELS];
|
||||
uint16_t erpm[IOMCU_MAX_TELEM_CHANNELS];
|
||||
uint32_t update_mask;
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue