Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: sync with master IOMCU firmware

Browse Source
This commit is contained in:
Andrew Tridgell 2019-07-08 10:37:04 +10:00
parent e2155ad02d
commit 5056fa68c7
12 changed files with 1935 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

377
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -14,109 +14,43 @@
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_ROMFS/AP_ROMFS.h> #include <AP_ROMFS/AP_ROMFS.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <SRV_Channel/SRV_Channel.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RCProtocol/AP_RCProtocol.h>
#include <AP_InternalError/AP_InternalError.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
#define PKT_MAX_REGS 32
//#define IOMCU_DEBUG
struct PACKED IOPacket {
uint8_t count:6;
uint8_t code:2;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
// get packet size in bytes
uint8_t get_size(void) const {
return count*2 + 4;
}
};
/*
values for pkt.code
*/
enum iocode {
// read types
CODE_READ = 0,
CODE_WRITE = 1,
// reply codes
CODE_SUCCESS = 0,
CODE_CORRUPT = 1,
CODE_ERROR = 2
};
// IO pages
enum iopage {
PAGE_CONFIG = 0,
PAGE_STATUS = 1,
PAGE_ACTUATORS = 2,
PAGE_SERVOS = 3,
PAGE_RAW_RCIN = 4,
PAGE_RCIN = 5,
PAGE_RAW_ADC = 6,
PAGE_PWM_INFO = 7,
PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_DISARMED_PWM = 108,
};
// pending IO events to send, used as an event mask // pending IO events to send, used as an event mask
enum ioevents { enum ioevents {
IOEVENT_INIT=1, IOEVENT_INIT=1,
IOEVENT_SEND_PWM_OUT, IOEVENT_SEND_PWM_OUT,
IOEVENT_SET_DISARMED_PWM,
IOEVENT_SET_FAILSAFE_PWM,
IOEVENT_FORCE_SAFETY_OFF, IOEVENT_FORCE_SAFETY_OFF,
IOEVENT_FORCE_SAFETY_ON, IOEVENT_FORCE_SAFETY_ON,
IOEVENT_SET_ONESHOT_ON, IOEVENT_SET_ONESHOT_ON,
IOEVENT_SET_BRUSHED_ON,
IOEVENT_SET_RATES, IOEVENT_SET_RATES,
IOEVENT_GET_RCIN,
IOEVENT_ENABLE_SBUS, IOEVENT_ENABLE_SBUS,
IOEVENT_SET_HEATER_TARGET, IOEVENT_SET_HEATER_TARGET,
IOEVENT_SET_DEFAULT_RATE, IOEVENT_SET_DEFAULT_RATE,
IOEVENT_SET_SAFETY_MASK, IOEVENT_SET_SAFETY_MASK,
IOEVENT_MIXING
}; };
// setup page registers // max number of consecutve protocol failures we accept before raising
#define PAGE_REG_SETUP_FEATURES 0 // an error
#define P_SETUP_FEATURES_SBUS1_OUT 1 #define IOMCU_MAX_REPEATED_FAILURES 20
#define P_SETUP_FEATURES_SBUS2_OUT 2
#define P_SETUP_FEATURES_PWM_RSSI 4
#define P_SETUP_FEATURES_ADC_RSSI 8
#define P_SETUP_FEATURES_ONESHOT 16
#define PAGE_REG_SETUP_ARMING 1
#define P_SETUP_ARMING_IO_ARM_OK (1<<0)
#define P_SETUP_ARMING_FMU_ARMED (1<<1)
#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6)
#define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on
#define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off
#define PAGE_REG_SETUP_PWM_RATE_MASK 2
#define PAGE_REG_SETUP_DEFAULTRATE 3
#define PAGE_REG_SETUP_ALTRATE 4
#define PAGE_REG_SETUP_REBOOT_BL 10
#define PAGE_REG_SETUP_CRC 11
#define PAGE_REG_SETUP_SBUS_RATE 19
#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
// magic value for rebooting to bootloader
#define REBOOT_BL_MAGIC 14662
#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12
#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14
#define FORCE_SAFETY_MAGIC 22027
AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) : AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) :
uart(_uart) uart(_uart)
{} {}
#if 0
#define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define debug(fmt, args ...)
#endif
/* /*
initialise library, starting thread initialise library, starting thread
*/ */
@ -127,18 +61,18 @@ void AP_IOMCU::init(void)
uart.set_blocking_writes(false); uart.set_blocking_writes(false);
uart.set_unbuffered_writes(true); uart.set_unbuffered_writes(true);
// check IO firmware CRC AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
hal.scheduler->delay(2000); if ((!boardconfig || boardconfig->io_enabled() == 1) && !hal.util->was_watchdog_reset()) {
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
if (!boardconfig || boardconfig->io_enabled() == 1) {
check_crc(); check_crc();
} else {
crc_is_ok = true;
} }
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU", if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) { 1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
AP_HAL::panic("Unable to allocate IOMCU thread"); AP_HAL::panic("Unable to allocate IOMCU thread");
} }
initialised = true;
} }
/* /*
@ -165,8 +99,8 @@ void AP_IOMCU::thread_main(void)
trigger_event(IOEVENT_INIT); trigger_event(IOEVENT_INIT);
while (true) { while (!do_shutdown) {
eventmask_t mask = chEvtWaitAnyTimeout(~0, MS2ST(10)); eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10));
// check for pending IO events // check for pending IO events
if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) { if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) {
@ -174,6 +108,14 @@ void AP_IOMCU::thread_main(void)
} }
if (mask & EVENT_MASK(IOEVENT_INIT)) { if (mask & EVENT_MASK(IOEVENT_INIT)) {
// get protocol version
if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) {
event_failed(IOEVENT_INIT);
continue;
}
is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION &&
config.protocol_version2 == IOMCU_PROTOCOL_VERSION2);
// set IO_ARM_OK and FMU_ARMED // set IO_ARM_OK and FMU_ARMED
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0, if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
P_SETUP_ARMING_IO_ARM_OK | P_SETUP_ARMING_IO_ARM_OK |
@ -184,7 +126,13 @@ void AP_IOMCU::thread_main(void)
} }
} }
if (mask & EVENT_MASK(IOEVENT_MIXING)) {
if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) {
event_failed(IOEVENT_MIXING);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) { if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
event_failed(IOEVENT_FORCE_SAFETY_OFF); event_failed(IOEVENT_FORCE_SAFETY_OFF);
@ -238,13 +186,20 @@ void AP_IOMCU::thread_main(void)
} }
} }
if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) {
event_failed(IOEVENT_SET_BRUSHED_ON);
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(IOEVENT_SET_SAFETY_MASK); event_failed(IOEVENT_SET_SAFETY_MASK);
continue; continue;
} }
} }
// check for regular timed events // check for regular timed events
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - last_rc_read_ms > 20) { if (now - last_rc_read_ms > 20) {
@ -280,7 +235,7 @@ void AP_IOMCU::thread_main(void)
// update safety pwm // update safety pwm
if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) { if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
uint8_t set = pwm_out.safety_pwm_set; uint8_t set = pwm_out.safety_pwm_set;
if (write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) { if (write_registers(PAGE_SAFETY_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
pwm_out.safety_pwm_sent = set; pwm_out.safety_pwm_sent = set;
} }
} }
@ -293,6 +248,7 @@ void AP_IOMCU::thread_main(void)
} }
} }
} }
done_shutdown = true;
} }
/* /*
@ -310,6 +266,8 @@ void AP_IOMCU::send_servo_out()
uint8_t n = pwm_out.num_channels; uint8_t n = pwm_out.num_channels;
if (rate.sbus_rate_hz == 0) { if (rate.sbus_rate_hz == 0) {
n = MIN(n, 8); n = MIN(n, 8);
} else {
n = MIN(n, IOMCU_MAX_CHANNELS);
} }
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) { if (now - last_servo_out_us >= 2000) {
@ -330,7 +288,7 @@ void AP_IOMCU::read_rc_input()
uint8_t n = MIN(MAX(9, rc_input.count), IOMCU_MAX_CHANNELS); uint8_t n = MIN(MAX(9, rc_input.count), IOMCU_MAX_CHANNELS);
read_registers(PAGE_RAW_RCIN, 0, 6+n, (uint16_t *)&rc_input); read_registers(PAGE_RAW_RCIN, 0, 6+n, (uint16_t *)&rc_input);
if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) { if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) {
rc_input.last_input_us = AP_HAL::micros(); rc_input.last_input_ms = AP_HAL::millis();
} }
} }
@ -342,13 +300,15 @@ void AP_IOMCU::read_status()
uint16_t *r = (uint16_t *)&reg_status; uint16_t *r = (uint16_t *)&reg_status;
read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r); read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r);
check_iomcu_reset();
if (reg_status.flag_safety_off == 0) { if (reg_status.flag_safety_off == 0) {
// if the IOMCU is indicating that safety is on, then force a // if the IOMCU is indicating that safety is on, then force a
// re-check of the safety options. This copes with a IOMCU reset // re-check of the safety options. This copes with a IOMCU reset
last_safety_options = 0xFFFF; last_safety_options = 0xFFFF;
// also check if the safety should be definately off. // also check if the safety should be definately off.
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance(); AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
if (!boardconfig) { if (!boardconfig) {
return; return;
} }
@ -391,6 +351,15 @@ void AP_IOMCU::discard_input(void)
*/ */
bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs) bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs)
{ {
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;
}
IOPacket pkt; IOPacket pkt;
discard_input(); discard_input();
@ -402,14 +371,21 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
pkt.page = page; pkt.page = page;
pkt.offset = offset; pkt.offset = offset;
pkt.crc = 0; pkt.crc = 0;
uint8_t pkt_size = pkt.get_size();
if (is_chibios_backend) {
// save bandwidth on reads
pkt_size = 4;
}
/* /*
the protocol is a bit strange, as it unnecessarily sends the the protocol is a bit strange, as it unnecessarily sends the
same size packet that it expects to receive. This means reading same size packet that it expects to receive. This means reading
a large number of registers wastes a lot of serial bandwidth a large number of registers wastes a lot of serial bandwidth
*/ */
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) { if (uart.write((uint8_t *)&pkt, pkt_size) != pkt_size) {
protocol_fail_count++;
return false; return false;
} }
@ -430,21 +406,29 @@ 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) {
hal.console->printf("bad crc %02x should be %02x n=%u %u/%u/%u\n", debug("bad crc %02x should be %02x n=%u %u/%u/%u\n",
got_crc, expected_crc, got_crc, expected_crc,
n, page, offset, count); n, page, offset, count);
protocol_fail_count++;
return false; return false;
} }
if (pkt.code != CODE_SUCCESS) { if (pkt.code != CODE_SUCCESS) {
hal.console->printf("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count); debug("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count);
protocol_fail_count++;
return false; return false;
} }
if (pkt.count < count) { if (pkt.count < count) {
hal.console->printf("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n); debug("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n);
protocol_fail_count++;
return false; return false;
} }
memcpy(regs, pkt.regs, count*2); memcpy(regs, pkt.regs, count*2);
if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
handle_repeated_failures();
}
protocol_fail_count = 0;
protocol_count++;
return true; return true;
} }
@ -453,6 +437,14 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
*/ */
bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs) bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
{ {
while (count > PKT_MAX_REGS) {
if (!write_registers(page, offset, PKT_MAX_REGS, regs)) {
return false;
}
offset += PKT_MAX_REGS;
count -= PKT_MAX_REGS;
regs += PKT_MAX_REGS;
}
IOPacket pkt; IOPacket pkt;
discard_input(); discard_input();
@ -467,12 +459,14 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
memcpy(pkt.regs, regs, 2*count); memcpy(pkt.regs, regs, 2*count);
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) { if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) {
protocol_fail_count++;
return false; return false;
} }
// wait for the expected number of reply bytes or timeout // wait for the expected number of reply bytes or timeout
if (!uart.wait_timeout(4, 10)) { if (!uart.wait_timeout(4, 10)) {
//hal.console->printf("no reply for %u/%u/%u\n", page, offset, count); //debug("no reply for %u/%u/%u\n", page, offset, count);
protocol_fail_count++;
return false; return false;
} }
@ -485,18 +479,25 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
} }
if (pkt.code != CODE_SUCCESS) { if (pkt.code != CODE_SUCCESS) {
hal.console->printf("bad code %02x write %u/%u/%u %02x/%02x n=%u\n", debug("bad code %02x write %u/%u/%u %02x/%02x n=%u\n",
pkt.code, page, offset, count, pkt.code, page, offset, count,
pkt.page, pkt.offset, n); pkt.page, pkt.offset, n);
protocol_fail_count++;
return false; return false;
} }
uint8_t got_crc = pkt.crc; uint8_t got_crc = pkt.crc;
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) {
hal.console->printf("bad crc %02x should be %02x\n", got_crc, expected_crc); debug("bad crc %02x should be %02x\n", got_crc, expected_crc);
protocol_fail_count++;
return false; return false;
} }
if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
handle_repeated_failures();
}
protocol_fail_count = 0;
protocol_count++;
return true; return true;
} }
@ -595,7 +596,7 @@ void AP_IOMCU::set_freq(uint16_t chmask, uint16_t freq)
{ {
const uint8_t masks[] = { 0x03,0x0C,0xF0 }; const uint8_t masks[] = { 0x03,0x0C,0xF0 };
// ensure mask is legal for the timer layout // ensure mask is legal for the timer layout
for (uint8_t i=0; i<ARRAY_SIZE_SIMPLE(masks); i++) { for (uint8_t i=0; i<ARRAY_SIZE(masks); i++) {
if (chmask & masks[i]) { if (chmask & masks[i]) {
chmask |= masks[i]; chmask |= masks[i];
} }
@ -627,10 +628,10 @@ 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) 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 != rc_input.last_input_us) { if (last_frame_us != uint32_t(rc_input.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_CHANNELS), max_chan);
memcpy(channels, rc_input.pwm, num_channels*2); memcpy(channels, rc_input.pwm, num_channels*2);
last_frame_us = rc_input.last_input_us; last_frame_us = uint32_t(rc_input.last_input_ms * 1000U);
return true; return true;
} }
return false; return false;
@ -658,10 +659,16 @@ void AP_IOMCU::set_oneshot_mode(void)
trigger_event(IOEVENT_SET_ONESHOT_ON); trigger_event(IOEVENT_SET_ONESHOT_ON);
} }
// setup for brushed mode
void AP_IOMCU::set_brushed_mode(void)
{
trigger_event(IOEVENT_SET_BRUSHED_ON);
}
// handling of BRD_SAFETYOPTION parameter // handling of BRD_SAFETYOPTION parameter
void AP_IOMCU::update_safety_options(void) void AP_IOMCU::update_safety_options(void)
{ {
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance(); AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
if (!boardconfig) { if (!boardconfig) {
return; return;
} }
@ -708,13 +715,20 @@ bool AP_IOMCU::check_crc(void)
} }
uint32_t io_crc = 0; uint32_t io_crc = 0;
if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc) && uint8_t tries = 32;
io_crc == crc) { while (tries--) {
if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc)) {
break;
}
}
if (io_crc == crc) {
hal.console->printf("IOMCU: CRC ok\n"); hal.console->printf("IOMCU: CRC ok\n");
crc_is_ok = true; crc_is_ok = true;
free(fw); free(fw);
fw = nullptr; fw = nullptr;
return true; return true;
} else {
hal.console->printf("IOMCU: CRC mismatch expected: 0x%X got: 0x%X\n", (unsigned)crc, (unsigned)io_crc);
} }
const uint16_t magic = REBOOT_BL_MAGIC; const uint16_t magic = REBOOT_BL_MAGIC;
@ -784,8 +798,145 @@ void AP_IOMCU::set_safety_mask(uint16_t chmask)
*/ */
bool AP_IOMCU::healthy(void) bool AP_IOMCU::healthy(void)
{ {
// for now just check CRC return crc_is_ok && protocol_fail_count == 0 && !detected_io_reset;
return crc_is_ok; }
/*
shutdown protocol, ready for reboot
*/
void AP_IOMCU::shutdown(void)
{
do_shutdown = true;
while (!done_shutdown) {
hal.scheduler->delay(1);
}
}
/*
request bind on a DSM radio
*/
void AP_IOMCU::bind_dsm(uint8_t mode)
{
if (!is_chibios_backend || hal.util->get_soft_armed()) {
// only with ChibiOS IO firmware, and disarmed
return;
}
uint16_t reg = mode;
write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSM_BIND, 1, &reg);
}
/*
setup for mixing. This allows fixed wing aircraft to fly in manual
mode if the FMU dies
*/
bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
float mixing_gain, uint16_t manual_rc_mask)
{
if (!is_chibios_backend) {
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
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
const SRV_Channel *c = SRV_Channels::srv_channel(i);
if (!c) {
continue;
}
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());
}
// update RCMap
MIX_UPDATE(mixing.rc_channel[0], rcmap->roll());
MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch());
MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle());
MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw());
for (uint8_t i=0; i<4; i++) {
const RC_Channel *c = RC_Channels::rc_channel(mixing.rc_channel[i]-1);
if (!c) {
continue;
}
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());
// cope with reversible throttle
if (i == 2 && c->get_type() == RC_Channel::RC_CHANNEL_TYPE_ANGLE) {
MIX_UPDATE(mixing.throttle_is_angle, 1);
} else {
MIX_UPDATE(mixing.throttle_is_angle, 0);
}
}
MIX_UPDATE(mixing.rc_chan_override, override_chan);
MIX_UPDATE(mixing.mixing_gain, (uint16_t)(mixing_gain*1000));
MIX_UPDATE(mixing.manual_rc_mask, manual_rc_mask);
// and enable
MIX_UPDATE(mixing.enabled, 1);
if (changed) {
trigger_event(IOEVENT_MIXING);
}
return true;
}
/*
return the RC protocol name
*/
const char *AP_IOMCU::get_rc_protocol(void)
{
if (!is_chibios_backend) {
return nullptr;
}
return AP_RCProtocol::protocol_name_from_protocol((AP_RCProtocol::rcprotocol_t)rc_input.data);
}
/*
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)
{
if (protocol_count < 100) {
// we're just starting up, ignore initial failures caused by
// initial sync with IOMCU
return;
}
AP::internalerror().error(AP_InternalError::error_t::iomcu_fail);
}
/*
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;
return;
}
uint32_t dt_ms = reg_status.timestamp_ms - last_iocmu_timestamp_ms;
last_iocmu_timestamp_ms = reg_status.timestamp_ms;
if (dt_ms < 500) {
// all OK
return;
}
detected_io_reset = true;
AP::internalerror().error(AP_InternalError::error_t::iomcu_reset);
hal.console->printf("IOMCU reset\n");
// 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);
trigger_event(IOEVENT_SET_DEFAULT_RATE);
} }
#endif // HAL_WITH_IO_MCU #endif // HAL_WITH_IO_MCU

88
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -10,8 +10,8 @@
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
#include "ch.h" #include "ch.h"
#include "iofirmware/ioprotocol.h"
#define IOMCU_MAX_CHANNELS 16 #include <AP_RCMapper/AP_RCMapper.h>
class AP_IOMCU { class AP_IOMCU {
public: public:
@ -65,6 +65,12 @@ public:
*/ */
bool check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_channels); bool check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_channels);
// Do DSM receiver binding
void bind_dsm(uint8_t mode);
// get the name of the RC protocol
const char *get_rc_protocol(void);
/* /*
get servo rail voltage get servo rail voltage
*/ */
@ -84,9 +90,19 @@ public:
// set to oneshot mode // set to oneshot mode
void set_oneshot_mode(void); void set_oneshot_mode(void);
// set to brushed mode
void set_brushed_mode(void);
// check if IO is healthy // check if IO is healthy
bool healthy(void); bool healthy(void);
// shutdown IO protocol (for reboot)
void shutdown();
// setup for FMU failsafe mixing
bool setup_mixing(RCMapper *rcmap, int8_t override_chan,
float mixing_gain, uint16_t manual_rc_mask);
private: private:
AP_HAL::UARTDriver &uart; AP_HAL::UARTDriver &uart;
@ -135,55 +151,18 @@ private:
void discard_input(void); void discard_input(void);
void event_failed(uint8_t event); void event_failed(uint8_t event);
void update_safety_options(void); void update_safety_options(void);
// CONFIG page
struct page_config config;
// PAGE_STATUS values // PAGE_STATUS values
struct PACKED { struct page_reg_status reg_status;
uint16_t freemem;
uint16_t cpuload;
// status flags
uint16_t flag_outputs_armed:1;
uint16_t flag_override:1;
uint16_t flag_rc_ok:1;
uint16_t flag_rc_ppm:1;
uint16_t flag_rc_dsm:1;
uint16_t flag_rc_sbus:1;
uint16_t flag_fmu_ok:1;
uint16_t flag_raw_pwm:1;
uint16_t flag_mixer_ok:1;
uint16_t flag_arm_sync:1;
uint16_t flag_init_ok:1;
uint16_t flag_failsafe:1;
uint16_t flag_safety_off:1;
uint16_t flag_fmu_initialised:1;
uint16_t flag_rc_st24:1;
uint16_t flag_rc_sumd_srxl:1;
uint16_t alarms;
uint16_t vbatt;
uint16_t ibatt;
uint16_t vservo;
uint16_t vrssi;
uint16_t prssi;
} reg_status;
// PAGE_RAW_RCIN values // PAGE_RAW_RCIN values
struct PACKED { struct page_rc_input rc_input;
uint16_t count;
uint16_t flags_frame_drop:1; // MIXER values
uint16_t flags_failsafe:1; struct page_mixing mixing;
uint16_t flags_dsm11:1;
uint16_t flags_mapping_ok:1;
uint16_t flags_rc_ok:1;
uint16_t flags_unused:11;
uint16_t nrssi;
uint16_t data;
uint16_t frame_count;
uint16_t lost_frame_count;
uint16_t pwm[IOMCU_MAX_CHANNELS];
uint16_t last_frame_count;
uint32_t last_input_us;
} rc_input;
// output pwm values // output pwm values
struct { struct {
@ -217,8 +196,17 @@ private:
uint32_t last_servo_out_us; uint32_t last_servo_out_us;
bool corked; bool corked;
bool do_shutdown;
bool done_shutdown;
bool crc_is_ok; bool crc_is_ok;
bool detected_io_reset;
bool initialised;
bool is_chibios_backend;
uint32_t protocol_fail_count;
uint32_t protocol_count;
uint32_t last_iocmu_timestamp_ms;
// firmware upload // firmware upload
const char *fw_name = "io_firmware.bin"; const char *fw_name = "io_firmware.bin";
@ -241,7 +229,9 @@ private:
bool reboot(); bool reboot();
bool check_crc(void); bool check_crc(void);
void handle_repeated_failures();
void check_iomcu_reset();
enum { enum {
PROTO_NOP = 0x00, PROTO_NOP = 0x00,
PROTO_OK = 0x10, PROTO_OK = 0x10,

16
libraries/AP_IOMCU/fw_uploader.cpp
View File

@ -287,20 +287,20 @@ bool AP_IOMCU::erase()
/* /*
send new firmware to bootloader send new firmware to bootloader
*/ */
bool AP_IOMCU::program(uint32_t fw_size) bool AP_IOMCU::program(uint32_t size)
{ {
bool ret = false; bool ret = false;
uint32_t sent = 0; uint32_t sent = 0;
if (fw_size & 3) { if (size & 3) {
return false; return false;
} }
debug("programming %u bytes...", (unsigned)fw_size); debug("programming %u bytes...", (unsigned)size);
while (sent < fw_size) { while (sent < size) {
/* get more bytes to program */ /* get more bytes to program */
uint32_t n = fw_size - sent; uint32_t n = size - sent;
if (n > PROG_MULTI_MAX) { if (n > PROG_MULTI_MAX) {
n = PROG_MULTI_MAX; n = PROG_MULTI_MAX;
} }
@ -325,7 +325,7 @@ bool AP_IOMCU::program(uint32_t fw_size)
/* /*
verify firmware for a rev2 bootloader verify firmware for a rev2 bootloader
*/ */
bool AP_IOMCU::verify_rev2(uint32_t fw_size) bool AP_IOMCU::verify_rev2(uint32_t size)
{ {
bool ret; bool ret;
size_t sent = 0; size_t sent = 0;
@ -339,9 +339,9 @@ bool AP_IOMCU::verify_rev2(uint32_t fw_size)
return ret; return ret;
} }
while (sent < fw_size) { while (sent < size) {
/* get more bytes to verify */ /* get more bytes to verify */
uint32_t n = fw_size - sent; uint32_t n = size - sent;
if (n > 4) { if (n > 4) {
n = 4; n = 4;
} }

88
libraries/AP_IOMCU/iofirmware/analog.cpp Normal file
View File

@ -0,0 +1,88 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
analog capture for IOMCU. This uses direct register access to avoid
using up a DMA channel and to minimise latency. We capture a single
sample at a time
*/
#include "ch.h"
#include "hal.h"
#include "analog.h"
#if HAL_USE_ADC != TRUE
#error "HAL_USE_ADC must be set"
#endif
/*
initialise ADC capture
*/
void adc_init(void)
{
adc_lld_init();
rccEnableADC1(true);
/* set channels 4 and 5 for 28.5us sample time */
ADC1->SMPR2 = ADC_SMPR2_SMP_AN4(ADC_SAMPLE_28P5) | ADC_SMPR2_SMP_AN5(ADC_SAMPLE_28P5);
/* capture a single sample at a time */
ADC1->SQR1 = 0;
ADC1->SQR2 = 0;
}
/*
capture one sample on a channel
*/
static uint16_t adc_sample_channel(uint32_t channel)
{
// clear EOC
ADC1->SR = 0;
/* capture one sample */
ADC1->SQR3 = channel;
ADC1->CR2 |= ADC_CR2_ADON;
/* wait for the conversion to complete */
uint32_t counter = 16;
while (!(ADC1->SR & ADC_SR_EOC)) {
if (--counter == 0) {
// ensure EOC is clear
ADC1->SR = 0;
return 0xffff;
}
}
// return sample (this also clears EOC flag)
return ADC1->DR;
}
/*
capture VSERVO in mV
*/
uint16_t adc_sample_vservo(void)
{
const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4);
return adc_sample_channel(channel) * 9900 / 4096;
}
/*
capture VRSSI in mV
*/
uint16_t adc_sample_vrssi(void)
{
const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5);
return adc_sample_channel(channel) * 9900 / 4096;
}

17
libraries/AP_IOMCU/iofirmware/analog.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
#include <stdint.h>
/*
initialise adc
*/
void adc_init(void);
/*
capture VSERVO
*/
uint16_t adc_sample_vservo(void);
/*
capture VRSSI
*/
uint16_t adc_sample_vrssi(void);

746
libraries/AP_IOMCU/iofirmware/iofirmware.cpp Normal file
View File

@ -0,0 +1,746 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
IOMCU main firmware
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include "iofirmware.h"
#include "hal.h"
#include <AP_HAL_ChibiOS/RCInput.h>
#include <AP_HAL_ChibiOS/RCOutput.h>
#include "analog.h"
#include "rc.h"
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
extern const AP_HAL::HAL &hal;
// we build this file with optimisation to lower the interrupt
// latency. This helps reduce the chance of losing an RC input byte
// due to missing a UART interrupt
#pragma GCC optimize("O3")
static AP_IOMCU_FW iomcu;
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// enable testing of IOMCU watchdog using safety switch
#define IOMCU_ENABLE_WATCHDOG_TEST 0
// pending events on the main thread
enum ioevents {
IOEVENT_PWM=1,
};
static struct {
uint32_t num_code_read, num_bad_crc, num_write_pkt, num_unknown_pkt;
uint32_t num_idle_rx, num_dma_complete_rx, num_total_rx, num_rx_error;
} stats;
static void dma_rx_end_cb(UARTDriver *uart)
{
osalSysLockFromISR();
uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
(void)uart->usart->SR;
(void)uart->usart->DR;
(void)uart->usart->DR;
dmaStreamDisable(uart->dmarx);
dmaStreamDisable(uart->dmatx);
iomcu.process_io_packet();
stats.num_total_rx++;
stats.num_dma_complete_rx = stats.num_total_rx - stats.num_idle_rx;
dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
dmaStreamSetMode(uart->dmarx, uart->dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmarx);
uart->usart->CR3 |= USART_CR3_DMAR;
dmaStreamSetMemory0(uart->dmatx, &iomcu.tx_io_packet);
dmaStreamSetTransactionSize(uart->dmatx, iomcu.tx_io_packet.get_size());
dmaStreamSetMode(uart->dmatx, uart->dmamode | STM32_DMA_CR_DIR_M2P |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmatx);
uart->usart->CR3 |= USART_CR3_DMAT;
osalSysUnlockFromISR();
}
static void idle_rx_handler(UARTDriver *uart)
{
volatile uint16_t sr = uart->usart->SR;
if (sr & (USART_SR_LBD | USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE |
USART_SR_PE)) { /* framing error - start/stop bit lost or line break */
/* send a line break - this will abort transmission/reception on the other end */
osalSysLockFromISR();
uart->usart->SR = ~USART_SR_LBD;
uart->usart->CR1 |= USART_CR1_SBK;
stats.num_rx_error++;
uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
(void)uart->usart->SR;
(void)uart->usart->DR;
(void)uart->usart->DR;
dmaStreamDisable(uart->dmarx);
dmaStreamDisable(uart->dmatx);
dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
dmaStreamSetMode(uart->dmarx, uart->dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmarx);
uart->usart->CR3 |= USART_CR3_DMAR;
osalSysUnlockFromISR();
return;
}
if (sr & USART_SR_IDLE) {
dma_rx_end_cb(uart);
stats.num_idle_rx++;
}
}
/*
* UART driver configuration structure.
*/
static UARTConfig uart_cfg = {
nullptr,
nullptr,
dma_rx_end_cb,
nullptr,
nullptr,
idle_rx_handler,
1500000, //1.5MBit
USART_CR1_IDLEIE,
0,
0
};
void setup(void)
{
hal.rcin->init();
hal.rcout->init();
for (uint8_t i = 0; i< 14; i++) {
hal.rcout->enable_ch(i);
}
iomcu.init();
iomcu.calculate_fw_crc();
uartStart(&UARTD2, &uart_cfg);
uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet);
}
void loop(void)
{
iomcu.update();
}
void AP_IOMCU_FW::init()
{
// the first protocol version must be 4 to allow downgrade to
// old NuttX based firmwares
config.protocol_version = IOMCU_PROTOCOL_VERSION;
config.protocol_version2 = IOMCU_PROTOCOL_VERSION2;
thread_ctx = chThdGetSelfX();
if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) {
has_heater = true;
}
adc_init();
rcin_serial_init();
// power on spektrum port
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
SPEKTRUM_POWER(1);
// we do no allocations after setup completes
reg_status.freemem = hal.util->available_memory();
if (hal.util->was_watchdog_safety_off()) {
hal.rcout->force_safety_off();
reg_status.flag_safety_off = true;
}
}
void AP_IOMCU_FW::update()
{
// we are not running any other threads, so we can use an
// immediate timeout here for lowest latency
eventmask_t mask = chEvtWaitAnyTimeout(~0, TIME_IMMEDIATE);
// we get the timestamp once here, and avoid fetching it
// within the DMA callbacks
last_ms = AP_HAL::millis();
loop_counter++;
if (do_reboot && (last_ms > reboot_time)) {
hal.scheduler->reboot(true);
while (true) {}
}
if ((mask & EVENT_MASK(IOEVENT_PWM)) ||
(last_safety_off != reg_status.flag_safety_off)) {
last_safety_off = reg_status.flag_safety_off;
pwm_out_update();
}
uint32_t now = last_ms;
reg_status.timestamp_ms = last_ms;
// output SBUS if enabled
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
reg_status.flag_safety_off &&
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);
}
// handle FMU failsafe
if (now - fmu_data_received_time > 200) {
// we are not getting input from the FMU. Fill in failsafe values at 100Hz
if (now - last_failsafe_ms > 10) {
fill_failsafe_pwm();
chEvtSignal(thread_ctx, EVENT_MASK(IOEVENT_PWM));
last_failsafe_ms = now;
}
// turn amber on
AMBER_SET(1);
} else {
last_failsafe_ms = now;
// turn amber off
AMBER_SET(0);
}
// update status page at 20Hz
if (now - last_status_ms > 50) {
last_status_ms = now;
page_status_update();
}
// run remaining functions at 1kHz
if (now != last_loop_ms) {
last_loop_ms = now;
heater_update();
rcin_update();
safety_update();
rcout_mode_update();
rcin_serial_update();
hal.rcout->timer_tick();
if (dsm_bind_state) {
dsm_bind_step();
}
}
}
void AP_IOMCU_FW::pwm_out_update()
{
memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
hal.rcout->cork();
for (uint8_t i = 0; i < SERVO_COUNT; i++) {
if (reg_status.flag_safety_off || (reg_setup.ignore_safety & (1U<<i))) {
hal.rcout->write(i, reg_servo.pwm[i]);
} else {
hal.rcout->write(i, 0);
}
}
hal.rcout->push();
}
void AP_IOMCU_FW::heater_update()
{
uint32_t now = last_ms;
if (!has_heater) {
// use blue LED as heartbeat, run it 4x faster when override active
if (now - last_blue_led_ms > (override_active?125:500)) {
BLUE_TOGGLE();
last_blue_led_ms = now;
}
} else if (reg_setup.heater_duty_cycle == 0 || (now - last_heater_ms > 3000UL)) {
// turn off the heater
HEATER_SET(0);
} else {
uint8_t cycle = ((now / 10UL) % 100U);
HEATER_SET(!(cycle >= reg_setup.heater_duty_cycle));
}
}
void AP_IOMCU_FW::rcin_update()
{
((ChibiOS::RCInput *)hal.rcin)->_timer_tick();
if (hal.rcin->new_input()) {
rc_input.count = hal.rcin->num_channels();
rc_input.flags_rc_ok = true;
for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS; i++) {
rc_input.pwm[i] = hal.rcin->read(i);
}
rc_input.last_input_ms = last_ms;
rc_input.data = (uint16_t)rcprotocol->protocol_detected();
} else if (last_ms - rc_input.last_input_ms > 200U) {
rc_input.flags_rc_ok = false;
}
if (update_rcout_freq) {
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
update_rcout_freq = false;
}
if (update_default_rate) {
hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
}
bool old_override = override_active;
// check for active override channel
if (mixing.enabled &&
mixing.rc_chan_override > 0 &&
rc_input.flags_rc_ok &&
mixing.rc_chan_override <= IOMCU_MAX_CHANNELS) {
override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1750);
} else {
override_active = false;
}
if (old_override != override_active) {
if (override_active) {
fill_failsafe_pwm();
}
chEvtSignal(thread_ctx, EVENT_MASK(IOEVENT_PWM));
}
}
void AP_IOMCU_FW::process_io_packet()
{
uint8_t rx_crc = rx_io_packet.crc;
uint8_t calc_crc;
rx_io_packet.crc = 0;
uint8_t pkt_size = rx_io_packet.get_size();
if (rx_io_packet.code == CODE_READ) {
// allow for more bandwidth efficient read packets
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, 4);
if (calc_crc != rx_crc) {
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
}
} else {
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
}
if (rx_crc != calc_crc || rx_io_packet.count > PKT_MAX_REGS) {
tx_io_packet.count = 0;
tx_io_packet.code = CODE_CORRUPT;
tx_io_packet.crc = 0;
tx_io_packet.page = 0;
tx_io_packet.offset = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
stats.num_bad_crc++;
return;
}
switch (rx_io_packet.code) {
case CODE_READ: {
stats.num_code_read++;
if (!handle_code_read()) {
tx_io_packet.count = 0;
tx_io_packet.code = CODE_ERROR;
tx_io_packet.crc = 0;
tx_io_packet.page = 0;
tx_io_packet.offset = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
}
}
break;
case CODE_WRITE: {
stats.num_write_pkt++;
if (!handle_code_write()) {
tx_io_packet.count = 0;
tx_io_packet.code = CODE_ERROR;
tx_io_packet.crc = 0;
tx_io_packet.page = 0;
tx_io_packet.offset = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
}
}
break;
default: {
stats.num_unknown_pkt++;
}
break;
}
}
/*
update dynamic elements of status page
*/
void AP_IOMCU_FW::page_status_update(void)
{
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) == 0) {
// we can only get VRSSI when sbus is disabled
reg_status.vrssi = adc_sample_vrssi();
} else {
reg_status.vrssi = 0;
}
reg_status.vservo = adc_sample_vservo();
}
bool AP_IOMCU_FW::handle_code_read()
{
uint16_t *values = nullptr;
#define COPY_PAGE(_page_name) \
do { \
values = (uint16_t *)&_page_name; \
tx_io_packet.count = sizeof(_page_name) / sizeof(uint16_t); \
} while(0);
switch (rx_io_packet.page) {
case PAGE_CONFIG:
COPY_PAGE(config);
break;
case PAGE_SETUP:
COPY_PAGE(reg_setup);
break;
case PAGE_RAW_RCIN:
COPY_PAGE(rc_input);
break;
case PAGE_STATUS:
COPY_PAGE(reg_status);
break;
case PAGE_SERVOS:
COPY_PAGE(reg_servo);
break;
default:
return false;
}
/* if the offset is at or beyond the end of the page, we have no data */
if (rx_io_packet.offset + rx_io_packet.count > tx_io_packet.count) {
return false;
}
/* correct the data pointer and count for the offset */
values += rx_io_packet.offset;
tx_io_packet.page = rx_io_packet.page;
tx_io_packet.offset = rx_io_packet.offset;
tx_io_packet.count -= rx_io_packet.offset;
tx_io_packet.count = MIN(tx_io_packet.count, rx_io_packet.count);
tx_io_packet.count = MIN(tx_io_packet.count, PKT_MAX_REGS);
tx_io_packet.code = CODE_SUCCESS;
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());
return true;
}
bool AP_IOMCU_FW::handle_code_write()
{
switch (rx_io_packet.page) {
case PAGE_SETUP:
switch (rx_io_packet.offset) {
case PAGE_REG_SETUP_ARMING:
reg_setup.arming = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_off();
reg_status.flag_safety_off = true;
} else {
return false;
}
break;
case PAGE_REG_SETUP_FORCE_SAFETY_ON:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_on();
reg_status.flag_safety_off = false;
} else {
return false;
}
break;
case PAGE_REG_SETUP_ALTRATE:
reg_setup.pwm_altrate = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_PWM_RATE_MASK:
reg_setup.pwm_rates = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_DEFAULTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
update_default_rate = true;
break;
case PAGE_REG_SETUP_SBUS_RATE:
reg_setup.sbus_rate = rx_io_packet.regs[0];
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
break;
case PAGE_REG_SETUP_FEATURES:
reg_setup.features = rx_io_packet.regs[0];
/* disable the conflicting options with SBUS 1 */
if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
P_SETUP_FEATURES_ADC_RSSI |
P_SETUP_FEATURES_SBUS2_OUT);
// enable SBUS output at specified rate
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
// we need to release the JTAG reset pin to be used as a GPIO, otherwise we can't enable
// or disable SBUS out
AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST;
palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
} else {
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = last_ms;
break;
case PAGE_REG_SETUP_REBOOT_BL:
if (reg_status.flag_safety_off) {
// don't allow reboot while armed
return false;
}
// check the magic value
if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
return false;
}
schedule_reboot(100);
break;
case PAGE_REG_SETUP_IGNORE_SAFETY:
reg_setup.ignore_safety = rx_io_packet.regs[0];
((ChibiOS::RCOutput *)hal.rcout)->set_safety_mask(reg_setup.ignore_safety);
break;
case PAGE_REG_SETUP_DSM_BIND:
if (dsm_bind_state == 0) {
dsm_bind_state = 1;
}
break;
default:
break;
}
break;
case PAGE_DIRECT_PWM: {
if (override_active) {
// no input when override is active
break;
}
/* copy channel data */
uint16_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(reg_direct_pwm.pwm)/2) {
return false;
}
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
}
offset++;
num_values--;
i++;
}
fmu_data_received_time = last_ms;
reg_status.flag_fmu_ok = true;
reg_status.flag_raw_pwm = true;
chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
break;
}
case PAGE_MIXING: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(mixing)/2) {
return false;
}
memcpy(((uint16_t *)&mixing)+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
case PAGE_SAFETY_PWM: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(reg_safety_pwm.pwm)/2) {
return false;
}
memcpy((&reg_safety_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
case PAGE_FAILSAFE_PWM: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(reg_failsafe_pwm.pwm)/2) {
return false;
}
memcpy((&reg_failsafe_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
default:
break;
}
tx_io_packet.count = 0;
tx_io_packet.code = CODE_SUCCESS;
tx_io_packet.crc = 0;
tx_io_packet.page = 0;
tx_io_packet.offset = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}
void AP_IOMCU_FW::schedule_reboot(uint32_t time_ms)
{
do_reboot = true;
reboot_time = last_ms + time_ms;
}
void AP_IOMCU_FW::calculate_fw_crc(void)
{
#define APP_SIZE_MAX 0xf000
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc_crc32(sum, (const uint8_t *)&bytes, sizeof(bytes));
}
reg_setup.crc[0] = sum & 0xFFFF;
reg_setup.crc[1] = sum >> 16;
}
/*
update safety state
*/
void AP_IOMCU_FW::safety_update(void)
{
uint32_t now = last_ms;
if (now - safety_update_ms < 100) {
// update safety at 10Hz
return;
}
safety_update_ms = now;
bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
if (safety_pressed) {
if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
safety_pressed = false;
} else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) {
safety_pressed = false;
}
}
if (safety_pressed) {
safety_button_counter++;
} else {
safety_button_counter = 0;
}
if (safety_button_counter == 10) {
// safety has been pressed for 1 second, change state
reg_status.flag_safety_off = !reg_status.flag_safety_off;
if (reg_status.flag_safety_off) {
hal.rcout->force_safety_off();
} else {
hal.rcout->force_safety_on();
}
}
#if IOMCU_ENABLE_WATCHDOG_TEST
if (safety_button_counter == 50) {
// deliberate lockup of IOMCU on 5s button press, for testing
// watchdog
while (true) {
hal.scheduler->delay(50);
palToggleLine(HAL_GPIO_PIN_SAFETY_LED);
if (palReadLine(HAL_GPIO_PIN_SAFETY_INPUT)) {
// only trigger watchdog on button release, so we
// don't end up stuck in the bootloader
stm32_watchdog_pat();
}
}
}
#endif
led_counter = (led_counter+1) % 16;
const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
}
/*
update hal.rcout mode if needed
*/
void AP_IOMCU_FW::rcout_mode_update(void)
{
bool use_oneshot = (reg_setup.features & P_SETUP_FEATURES_ONESHOT) != 0;
if (use_oneshot && !oneshot_enabled) {
oneshot_enabled = true;
hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
}
bool use_brushed = (reg_setup.features & P_SETUP_FEATURES_BRUSHED) != 0;
if (use_brushed && !brushed_enabled) {
brushed_enabled = true;
if (reg_setup.pwm_rates == 0) {
// default to 2kHz for all channels for brushed output
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_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
}
}
/*
fill in failsafe PWM values
*/
void AP_IOMCU_FW::fill_failsafe_pwm(void)
{
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
if (reg_status.flag_safety_off) {
reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i];
} else {
reg_direct_pwm.pwm[i] = reg_safety_pwm.pwm[i];
}
}
if (mixing.enabled) {
run_mixer();
}
}
AP_HAL_MAIN();

152
libraries/AP_IOMCU/iofirmware/iofirmware.h Normal file
View File

@ -0,0 +1,152 @@
#pragma once
#include <stdint.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_RCProtocol/AP_RCProtocol.h>
#include "ch.h"
#include "ioprotocol.h"
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
#define SERVO_COUNT 8
class AP_IOMCU_FW {
public:
void process_io_packet();
struct IOPacket rx_io_packet, tx_io_packet;
void init();
void update();
void calculate_fw_crc(void);
private:
void pwm_out_update();
void heater_update();
void rcin_update();
bool handle_code_write();
bool handle_code_read();
void schedule_reboot(uint32_t time_ms);
void safety_update();
void rcout_mode_update();
void rcin_serial_init();
void rcin_serial_update();
void page_status_update(void);
void fill_failsafe_pwm(void);
void run_mixer(void);
int16_t mix_input_angle(uint8_t channel, uint16_t radio_in) const;
int16_t mix_input_range(uint8_t channel, uint16_t radio_in) const;
uint16_t mix_output_angle(uint8_t channel, int16_t angle) const;
uint16_t mix_output_range(uint8_t channel, int16_t value) const;
int16_t mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const;
void dsm_bind_step(void);
struct PACKED {
/* default to RSSI ADC functionality */
uint16_t features;
uint16_t arming;
uint16_t pwm_rates;
uint16_t pwm_defaultrate = 50;
uint16_t pwm_altrate = 200;
uint16_t relays_pad;
uint16_t vbatt_scale = 10000;
uint16_t reserved1;
uint16_t reserved2;
uint16_t set_debug;
uint16_t reboot_bl;
uint16_t crc[2];
uint16_t rc_thr_failsafe_us;
uint16_t reserved3;
uint16_t pwm_reverse;
uint16_t trim_roll;
uint16_t trim_pitch;
uint16_t trim_yaw;
uint16_t sbus_rate = 72;
uint16_t ignore_safety;
uint16_t heater_duty_cycle = 0xFFFFU;
uint16_t pwm_altclock = 1;
} reg_setup;
// CONFIG values
struct page_config config;
// PAGE_STATUS values
struct page_reg_status reg_status;
// PAGE_RAW_RCIN values
struct page_rc_input rc_input;
// PAGE_SERVO values
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS];
} reg_servo;
// PAGE_DIRECT_PWM values
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS];
} reg_direct_pwm;
// PAGE_FAILSAFE_PWM
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS];
} reg_failsafe_pwm;
// PAGE_SAFETY_PWM
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS];
} reg_safety_pwm;
// output rates
struct {
uint16_t freq;
uint16_t chmask;
uint16_t default_freq = 50;
uint16_t sbus_rate_hz;
} rate;
// MIXER values
struct page_mixing mixing;
// true when override channel active
bool override_active;
// sbus rate handling
uint32_t sbus_last_ms;
uint32_t sbus_interval_ms;
AP_RCProtocol *rcprotocol;
uint32_t fmu_data_received_time;
uint32_t last_heater_ms;
uint32_t reboot_time;
bool do_reboot;
bool update_default_rate;
bool update_rcout_freq;
bool has_heater;
uint32_t last_blue_led_ms;
uint32_t safety_update_ms;
uint32_t safety_button_counter;
uint8_t led_counter;
uint32_t last_loop_ms;
bool oneshot_enabled;
bool brushed_enabled;
thread_t *thread_ctx;
bool last_safety_off;
uint32_t last_status_ms;
uint32_t last_ms;
uint32_t loop_counter;
uint8_t dsm_bind_state;
uint32_t last_dsm_bind_ms;
uint32_t last_failsafe_ms;
};
// GPIO macros
#define HEATER_SET(on) palWriteLine(HAL_GPIO_PIN_HEATER, !(on));
#define BLUE_TOGGLE() palToggleLine(HAL_GPIO_PIN_HEATER);
#define AMBER_SET(on) palWriteLine(HAL_GPIO_PIN_AMBER_LED, !(on));
#define SPEKTRUM_POWER(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, on);
#define SPEKTRUM_SET(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_OUT, on);

185
libraries/AP_IOMCU/iofirmware/ioprotocol.h Normal file
View File

@ -0,0 +1,185 @@
#pragma once
#include <stdint.h>
#include <AP_Common/AP_Common.h>
/*
common protocol definitions between AP_IOMCU and iofirmware
*/
// 22 is enough for the rc_input page in one transfer
#define PKT_MAX_REGS 22
#define IOMCU_MAX_CHANNELS 16
//#define IOMCU_DEBUG
struct PACKED IOPacket {
uint8_t count:6;
uint8_t code:2;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
// get packet size in bytes
uint8_t get_size(void) const
{
return count*2 + 4;
}
};
/*
values for pkt.code
*/
enum iocode {
// read types
CODE_READ = 0,
CODE_WRITE = 1,
// reply codes
CODE_SUCCESS = 0,
CODE_CORRUPT = 1,
CODE_ERROR = 2
};
// IO pages
enum iopage {
PAGE_CONFIG = 0,
PAGE_STATUS = 1,
PAGE_ACTUATORS = 2,
PAGE_SERVOS = 3,
PAGE_RAW_RCIN = 4,
PAGE_RCIN = 5,
PAGE_RAW_ADC = 6,
PAGE_PWM_INFO = 7,
PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_SAFETY_PWM = 108,
PAGE_MIXING = 200,
};
// setup page registers
#define PAGE_REG_SETUP_FEATURES 0
#define P_SETUP_FEATURES_SBUS1_OUT 1
#define P_SETUP_FEATURES_SBUS2_OUT 2
#define P_SETUP_FEATURES_PWM_RSSI 4
#define P_SETUP_FEATURES_ADC_RSSI 8
#define P_SETUP_FEATURES_ONESHOT 16
#define P_SETUP_FEATURES_BRUSHED 32
#define PAGE_REG_SETUP_ARMING 1
#define P_SETUP_ARMING_IO_ARM_OK (1<<0)
#define P_SETUP_ARMING_FMU_ARMED (1<<1)
#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6)
#define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on
#define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off
#define PAGE_REG_SETUP_PWM_RATE_MASK 2
#define PAGE_REG_SETUP_DEFAULTRATE 3
#define PAGE_REG_SETUP_ALTRATE 4
#define PAGE_REG_SETUP_REBOOT_BL 10
#define PAGE_REG_SETUP_CRC 11
#define PAGE_REG_SETUP_SBUS_RATE 19
#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
#define PAGE_REG_SETUP_DSM_BIND 22
// config page registers
#define PAGE_CONFIG_PROTOCOL_VERSION 0
#define PAGE_CONFIG_PROTOCOL_VERSION2 1
#define IOMCU_PROTOCOL_VERSION 4
#define IOMCU_PROTOCOL_VERSION2 10
// magic value for rebooting to bootloader
#define REBOOT_BL_MAGIC 14662
#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12
#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14
#define FORCE_SAFETY_MAGIC 22027
struct PACKED page_config {
uint16_t protocol_version;
uint16_t protocol_version2;
};
struct PACKED page_reg_status {
uint16_t freemem;
uint16_t cpuload;
// status flags
uint16_t flag_outputs_armed:1;
uint16_t flag_override:1;
uint16_t flag_rc_ok:1;
uint16_t flag_rc_ppm:1;
uint16_t flag_rc_dsm:1;
uint16_t flag_rc_sbus:1;
uint16_t flag_fmu_ok:1;
uint16_t flag_raw_pwm:1;
uint16_t flag_mixer_ok:1;
uint16_t flag_arm_sync:1;
uint16_t flag_init_ok:1;
uint16_t flag_failsafe:1;
uint16_t flag_safety_off:1;
uint16_t flag_fmu_initialised:1;
uint16_t flag_rc_st24:1;
uint16_t flag_rc_sumd_srxl:1;
uint16_t alarms;
uint32_t timestamp_ms;
uint16_t vservo;
uint16_t vrssi;
uint16_t prssi;
};
struct PACKED page_rc_input {
uint16_t count;
uint16_t flags_frame_drop:1;
uint16_t flags_failsafe:1;
uint16_t flags_dsm11:1;
uint16_t flags_mapping_ok:1;
uint16_t flags_rc_ok:1;
uint16_t flags_unused:11;
uint16_t nrssi;
uint16_t data;
uint16_t frame_count;
uint16_t lost_frame_count;
uint16_t pwm[IOMCU_MAX_CHANNELS];
// the following two fields are not transferred to the FMU
uint16_t last_frame_count;
uint32_t last_input_ms;
};
/*
data for mixing on FMU failsafe
*/
struct PACKED page_mixing {
uint16_t servo_min[IOMCU_MAX_CHANNELS];
uint16_t servo_max[IOMCU_MAX_CHANNELS];
uint16_t servo_trim[IOMCU_MAX_CHANNELS];
uint8_t servo_function[IOMCU_MAX_CHANNELS];
uint8_t servo_reversed[IOMCU_MAX_CHANNELS];
// RC input arrays are in AETR order
uint16_t rc_min[4];
uint16_t rc_max[4];
uint16_t rc_trim[4];
uint8_t rc_reversed[IOMCU_MAX_CHANNELS];
uint8_t rc_channel[4];
// gain for elevon and vtail mixing, x1000
uint16_t mixing_gain;
// channel which when high forces mixer
int8_t rc_chan_override;
// is the throttle an angle input?
uint8_t throttle_is_angle;
// mask of channels which are pure manual in override
uint16_t manual_rc_mask;
// enabled needs to be 1 to enable mixing
uint8_t enabled;
uint8_t pad; // pad to even size
};

221
libraries/AP_IOMCU/iofirmware/mixer.cpp Normal file
View File

@ -0,0 +1,221 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
mixer for failsafe operation when FMU is dead
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <SRV_Channel/SRV_Channel.h>
#include "iofirmware.h"
#define ANGLE_SCALE ((int32_t)4500)
#define RANGE_SCALE ((int32_t)1000)
/*
return a RC input value scaled from -4500 to 4500
*/
int16_t AP_IOMCU_FW::mix_input_angle(uint8_t channel, uint16_t radio_in) const
{
const uint16_t &rc_min = mixing.rc_min[channel];
const uint16_t &rc_max = mixing.rc_max[channel];
const uint16_t &rc_trim = mixing.rc_trim[channel];
const uint16_t &reversed = mixing.rc_reversed[channel];
int16_t ret = 0;
if (radio_in > rc_trim && rc_max != rc_trim) {
ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_max - rc_trim);
} else if (radio_in < rc_trim && rc_trim != rc_min) {
ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_trim - rc_min);
}
if (reversed) {
ret = -ret;
}
return ret;
}
/*
return a RC input value scaled from 0 to 1000
*/
int16_t AP_IOMCU_FW::mix_input_range(uint8_t channel, uint16_t radio_in) const
{
const uint16_t &rc_min = mixing.rc_min[channel];
const uint16_t &rc_max = mixing.rc_max[channel];
const uint16_t &reversed = mixing.rc_reversed[channel];
int16_t ret = 0;
if (radio_in > rc_max) {
ret = RANGE_SCALE;
} else if (radio_in < rc_min) {
ret = -RANGE_SCALE;
} else {
ret = (RANGE_SCALE * (int32_t)(radio_in - rc_min)) / (int32_t)(rc_max - rc_min);
}
if (reversed) {
ret = -ret;
}
return ret;
}
/*
return an output pwm giving an angle for a servo channel
*/
uint16_t AP_IOMCU_FW::mix_output_angle(uint8_t channel, int16_t angle) const
{
const uint16_t &srv_min = mixing.servo_min[channel];
const uint16_t &srv_max = mixing.servo_max[channel];
const uint16_t &srv_trim = mixing.servo_trim[channel];
const uint16_t &reversed = mixing.servo_reversed[channel];
if (reversed) {
angle = -angle;
}
angle = constrain_int16(angle, -ANGLE_SCALE, ANGLE_SCALE);
if (angle > 0) {
return srv_trim + ((int32_t)angle * (int32_t)(srv_max - srv_trim)) / ANGLE_SCALE;
}
return srv_trim - (-(int32_t)angle * (int32_t)(srv_trim - srv_min)) / ANGLE_SCALE;
}
/*
return an output pwm giving an range for a servo channel
*/
uint16_t AP_IOMCU_FW::mix_output_range(uint8_t channel, int16_t value) const
{
const uint16_t &srv_min = mixing.servo_min[channel];
const uint16_t &srv_max = mixing.servo_max[channel];
const uint16_t &reversed = mixing.servo_reversed[channel];
value = constrain_int16(value, 0, RANGE_SCALE);
if (reversed) {
value = RANGE_SCALE - value;
}
return srv_min + ((int32_t)value * (int32_t)(srv_max - srv_min)) / RANGE_SCALE;
}
/*
elevon and vtail mixer
*/
int16_t AP_IOMCU_FW::mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const
{
if (first_output) {
return (angle2 - angle1) * mixing.mixing_gain / 1000;
}
return (angle1 + angle2) * mixing.mixing_gain / 1000;
}
/*
run mixer. This is used when FMU is not providing inputs, or when
the OVERRIDE_CHAN is high. It allows for manual fixed wing flight
*/
void AP_IOMCU_FW::run_mixer(void)
{
int16_t rcin[4] = {0, 0, 0, 0};
int16_t &roll = rcin[0];
int16_t &pitch = rcin[1];
int16_t &throttle = rcin[2];
int16_t &rudder = rcin[3];
// get RC input angles
if (rc_input.flags_rc_ok) {
for (uint8_t i=0;i<4; i++) {
if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_CHANNELS) {
uint8_t chan = mixing.rc_channel[i]-1;
if (i == 2 && !mixing.throttle_is_angle) {
rcin[i] = mix_input_range(i, rc_input.pwm[chan]);
} else {
rcin[i] = mix_input_angle(i, rc_input.pwm[chan]);
}
}
}
}
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i];
uint16_t &pwm = reg_direct_pwm.pwm[i];
if (mixing.manual_rc_mask & (1U<<i)) {
// treat as pass-thru if this channel is set in MANUAL_RC_MASK
function = SRV_Channel::k_manual;
}
switch (function) {
case SRV_Channel::k_manual:
pwm = rc_input.pwm[i];
break;
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16:
pwm = rc_input.pwm[(uint8_t)(function - SRV_Channel::k_rcin1)];
break;
case SRV_Channel::k_aileron:
case SRV_Channel::k_aileron_with_input:
case SRV_Channel::k_flaperon_left:
case SRV_Channel::k_flaperon_right:
pwm = mix_output_angle(i, roll);
break;
case SRV_Channel::k_elevator:
case SRV_Channel::k_elevator_with_input:
pwm = mix_output_angle(i, pitch);
break;
case SRV_Channel::k_rudder:
case SRV_Channel::k_steering:
pwm = mix_output_angle(i, rudder);
break;
case SRV_Channel::k_throttle:
case SRV_Channel::k_throttleLeft:
case SRV_Channel::k_throttleRight:
if (mixing.throttle_is_angle) {
pwm = mix_output_angle(i, throttle);
} else {
pwm = mix_output_range(i, throttle);
}
break;
case SRV_Channel::k_flap:
case SRV_Channel::k_flap_auto:
// zero flaps
pwm = mix_output_range(i, 0);
break;
case SRV_Channel::k_elevon_left:
case SRV_Channel::k_dspoilerLeft1:
case SRV_Channel::k_dspoilerLeft2:
// treat differential spoilers as elevons
pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, true));
break;
case SRV_Channel::k_elevon_right:
case SRV_Channel::k_dspoilerRight1:
case SRV_Channel::k_dspoilerRight2:
// treat differential spoilers as elevons
pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, false));
break;
case SRV_Channel::k_vtail_left:
pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, false));
break;
case SRV_Channel::k_vtail_right:
pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, true));
break;
default:
break;
}
}
}

186
libraries/AP_IOMCU/iofirmware/rc.cpp Normal file
View File

@ -0,0 +1,186 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
SBUS output support
*/
#include "ch.h"
#include "hal.h"
#include "iofirmware.h"
#include "rc.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_SBusOut/AP_SBusOut.h>
extern const AP_HAL::HAL& hal;
// usart3 is for SBUS input and output
static const SerialConfig uart3_cfg = {
100000, // speed
USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
USART_CR2_STOP_1, // cr2, two stop bits
0, // cr3
nullptr, // irq_cb
nullptr, // ctx
};
// listen for parity errors on sd3 input
static event_listener_t sd3_listener;
void sbus_out_write(uint16_t *channels, uint8_t nchannels)
{
uint8_t buffer[25];
AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
chnWrite(&SD3, buffer, sizeof(buffer));
}
// usart1 is for DSM input and (optionally) debug to FMU
static const SerialConfig uart1_cfg = {
115200, // speed
0, // cr1
0, // cr2
0, // cr3
nullptr, // irq_cb
nullptr, // ctx
};
/*
init rcin on DSM USART1
*/
void AP_IOMCU_FW::rcin_serial_init(void)
{
sdStart(&SD1, &uart1_cfg);
sdStart(&SD3, &uart3_cfg);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
&sd3_listener,
EVENT_MASK(1),
SD_PARITY_ERROR);
rcprotocol = AP_RCProtocol::get_singleton();
// disable input for SBUS with pulses, we will use the UART for
// SBUS.
rcprotocol->disable_for_pulses(AP_RCProtocol::SBUS);
rcprotocol->disable_for_pulses(AP_RCProtocol::SBUS_NI);
}
static struct {
uint32_t num_dsm_bytes;
uint32_t num_sbus_bytes;
uint32_t num_sbus_errors;
eventflags_t sbus_error;
} rc_stats;
/*
check for data on DSM RX uart
*/
void AP_IOMCU_FW::rcin_serial_update(void)
{
uint8_t b[16];
uint32_t n;
// read from DSM port
if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
n = MIN(n, sizeof(b));
rc_stats.num_dsm_bytes += n;
for (uint8_t i=0; i<n; i++) {
rcprotocol->process_byte(b[i], 115200);
}
//BLUE_TOGGLE();
}
// read from SBUS port
if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
eventflags_t flags;
if ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR) {
rc_stats.sbus_error = flags;
rc_stats.num_sbus_errors++;
} else {
n = MIN(n, sizeof(b));
rc_stats.num_sbus_bytes += n;
for (uint8_t i=0; i<n; i++) {
rcprotocol->process_byte(b[i], 100000);
}
}
}
}
/*
sleep for 1ms using a busy loop
*/
static void delay_one_ms(uint32_t &now)
{
while (now == AP_HAL::millis()) ;
now = AP_HAL::millis();
}
/*
perform a DSM bind operation
*/
void AP_IOMCU_FW::dsm_bind_step(void)
{
uint32_t now = last_ms;
switch (dsm_bind_state) {
case 1:
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
SPEKTRUM_POWER(0);
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
SPEKTRUM_SET(1);
dsm_bind_state = 2;
last_dsm_bind_ms = now;
break;
case 2:
if (now - last_dsm_bind_ms >= 500) {
SPEKTRUM_POWER(1);
dsm_bind_state = 3;
last_dsm_bind_ms = now;
}
break;
case 3: {
if (now - last_dsm_bind_ms >= 72) {
// 9 pulses works with all satellite receivers, and supports the highest
// available protocol
delay_one_ms(now);
const uint8_t num_pulses = 9;
for (uint8_t i=0; i<num_pulses; i++) {
// the delay should be 120us, but we are running our
// clock at 1kHz, and 1ms works fine
delay_one_ms(now);
SPEKTRUM_SET(0);
delay_one_ms(now);
SPEKTRUM_SET(1);
}
last_dsm_bind_ms = now;
dsm_bind_state = 4;
}
break;
}
case 4:
if (now - last_dsm_bind_ms >= 50) {
// set back as input pin
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_INPUT);
dsm_bind_state = 0;
}
break;
default:
dsm_bind_state = 0;
break;
}
}

4
libraries/AP_IOMCU/iofirmware/rc.h Normal file
View File

@ -0,0 +1,4 @@
#pragma once
#include <stdint.h>
void sbus_out_write(uint16_t *channels, uint8_t nchannels);

25
libraries/AP_IOMCU/iofirmware/wscript Normal file
View File

@ -0,0 +1,25 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_stlib(
name= 'iofirmware_libs',
ap_vehicle='iofirmware',
ap_libraries= [
'AP_Common',
'AP_HAL',
'AP_HAL_Empty',
'AP_Math',
'AP_RCProtocol',
'AP_BoardConfig',
'AP_SBusOut'
],
exclude_src=[
'libraries/AP_HAL_ChibiOS/Storage.cpp'
]
)
bld.ap_program(
program_name='iofirmware',
use='iofirmware_libs',
program_groups=['bin','iofirmware'],
)