Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: added mixing structure to protocol

Browse Source
This commit is contained in:
Andrew Tridgell 2018-10-31 11:07:47 +11:00
parent 8d5b9521ad
commit 191f72d2e6
5 changed files with 175 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -14,6 +14,8 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_ROMFS/AP_ROMFS.h>
#include <AP_Math/crc.h>
#include <SRV_Channel/SRV_Channel.h>
#include <RC_Channel/RC_Channel.h>
extern const AP_HAL::HAL &hal;
@ -32,12 +34,19 @@ enum ioevents {
IOEVENT_SET_HEATER_TARGET,
IOEVENT_SET_DEFAULT_RATE,
IOEVENT_SET_SAFETY_MASK,
IOEVENT_MIXING
};
AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_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
*/
@ -54,12 +63,15 @@ void AP_IOMCU::init(void)
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
if (!boardconfig || boardconfig->io_enabled() == 1) {
check_crc();
} else {
crc_is_ok = true;
}
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
AP_HAL::panic("Unable to allocate IOMCU thread");
}
initialised = true;
}
/*
@ -95,6 +107,13 @@ void AP_IOMCU::thread_main(void)
}
if (mask & EVENT_MASK(IOEVENT_INIT)) {
// get protocol version
if (!read_registers(PAGE_CONFIG, PAGE_CONFIG_PROTOCOL_VERSION, sizeof(config)/2, (uint16_t *)&config)) {
event_failed(IOEVENT_INIT);
continue;
}
debug("io protocol: %u\n", config.protocol_version);
// set IO_ARM_OK and FMU_ARMED
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
P_SETUP_ARMING_IO_ARM_OK |
@ -105,6 +124,12 @@ 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 (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
@ -232,6 +257,8 @@ void AP_IOMCU::send_servo_out()
uint8_t n = pwm_out.num_channels;
if (rate.sbus_rate_hz == 0) {
n = MIN(n, 8);
} else {
n = MIN(n, IOMCU_MAX_CHANNELS);
}
uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) {
@ -313,6 +340,15 @@ void AP_IOMCU::discard_input(void)
*/
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;
discard_input();
@ -332,6 +368,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
*/
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) {
protocol_fail_count++;
return false;
}
@ -352,21 +389,25 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
pkt.crc = 0;
uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
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,
n, page, offset, count);
protocol_fail_count++;
return false;
}
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;
}
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;
}
memcpy(regs, pkt.regs, count*2);
protocol_fail_count = 0;
return true;
}
@ -375,6 +416,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)
{
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;
discard_input();
@ -389,12 +438,14 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
memcpy(pkt.regs, regs, 2*count);
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) {
protocol_fail_count++;
return false;
}
// wait for the expected number of reply bytes or timeout
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;
}
@ -407,18 +458,21 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
}
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.page, pkt.offset, n);
protocol_fail_count++;
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) {
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;
}
protocol_fail_count = 0;
return true;
}
@ -713,8 +767,7 @@ void AP_IOMCU::set_safety_mask(uint16_t chmask)
*/
bool AP_IOMCU::healthy(void)
{
// for now just check CRC
return crc_is_ok;
return crc_is_ok && protocol_fail_count == 0;
}
/*
@ -728,4 +781,53 @@ void AP_IOMCU::shutdown(void)
}
}
/*
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)
{
if (config.protocol_version != IOMCU_PROTOCOL_VERSION) {
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 *ch = SRV_Channels::srv_channel(i);
if (!ch) {
continue;
}
MIX_UPDATE(mixing.servo_trim[i], ch->get_trim());
MIX_UPDATE(mixing.servo_min[i], ch->get_output_min());
MIX_UPDATE(mixing.servo_max[i], ch->get_output_max());
MIX_UPDATE(mixing.servo_function[i], ch->get_function());
}
// 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++) {
if (mixing.rc_channel[i] <= 0 || mixing.rc_channel[i] >= NUM_RC_CHANNELS) {
continue;
}
const RC_Channel *ch = RC_Channels::rc_channel(mixing.rc_channel[i]-1);
if (!ch) {
continue;
}
MIX_UPDATE(mixing.rc_min[i], ch->get_radio_min());
MIX_UPDATE(mixing.rc_max[i], ch->get_radio_max());
MIX_UPDATE(mixing.rc_trim[i], ch->get_radio_trim());
}
MIX_UPDATE(mixing.rc_chan_override, override_chan);
// and enable
MIX_UPDATE(mixing.enabled, 1);
if (changed) {
trigger_event(IOEVENT_MIXING);
}
return true;
}
#endif // HAL_WITH_IO_MCU

13
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -11,6 +11,7 @@
#include "ch.h"
#include "iofirmware/ioprotocol.h"
#include <AP_RCMapper/AP_RCMapper.h>
class AP_IOMCU {
public:
@ -89,6 +90,9 @@ public:
// shutdown IO protocol (for reboot)
void shutdown();
// setup for FMU failsafe mixing
bool setup_mixing(RCMapper *rcmap, int8_t override_chan);
private:
AP_HAL::UARTDriver &uart;
@ -138,12 +142,18 @@ private:
void event_failed(uint8_t event);
void update_safety_options(void);
// CONFIG page
struct page_config config;
// PAGE_STATUS values
struct page_reg_status reg_status;
// PAGE_RAW_RCIN values
struct page_rc_input rc_input;
// MIXER values
struct page_mixing mixing;
// output pwm values
struct {
uint8_t num_channels;
@ -180,6 +190,9 @@ private:
bool done_shutdown;
bool crc_is_ok;
bool initialised;
uint32_t protocol_fail_count;
// firmware upload
const char *fw_name = "io_firmware.bin";

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

@ -326,6 +326,9 @@ bool AP_IOMCU_FW::handle_code_read()
} while(0);
switch (rx_io_packet.page) {
case PAGE_CONFIG:
COPY_PAGE(config);
break;
case PAGE_SETUP:
COPY_PAGE(reg_setup);
break;
@ -461,6 +464,11 @@ bool AP_IOMCU_FW::handle_code_write()
chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
break;
}
case PAGE_MIXING: {
uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
memcpy(((uint16_t *)&mixing)+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
default:
break;

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

@ -4,7 +4,6 @@
#include "ch.h"
#include "ioprotocol.h"
#define IOMCU_MAX_CHANNELS 16
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
#define SERVO_COUNT 8
@ -56,6 +55,9 @@ private:
uint16_t pwm_altclock = 1;
} reg_setup;
// CONFIG values
struct page_config config;
// PAGE_STATUS values
struct page_reg_status reg_status;
@ -80,6 +82,9 @@ private:
uint16_t sbus_rate_hz;
} rate;
// MIXER values
struct page_mixing mixing;
// sbus rate handling
uint32_t sbus_last_ms;
uint32_t sbus_interval_ms;

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

@ -50,6 +50,7 @@ enum iopage {
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_DISARMED_PWM = 108,
PAGE_MIXING = 200,
};
// setup page registers
@ -77,6 +78,10 @@ enum iopage {
#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
// config page registers
#define PAGE_CONFIG_PROTOCOL_VERSION 0
#define IOMCU_PROTOCOL_VERSION 10
// magic value for rebooting to bootloader
#define REBOOT_BL_MAGIC 14662
@ -84,6 +89,10 @@ enum iopage {
#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14
#define FORCE_SAFETY_MAGIC 22027
struct PACKED page_config {
uint16_t protocol_version = IOMCU_PROTOCOL_VERSION;
};
struct PACKED page_reg_status {
uint16_t freemem;
uint16_t cpuload;
@ -130,3 +139,25 @@ struct PACKED page_rc_input {
uint16_t last_frame_count;
uint32_t last_input_us;
};
/*
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];
// 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_channel[4];
// channel which when high forces mixer
int8_t rc_chan_override;
// enabled needs to be 1 to enable mixing
uint8_t enabled;
};