mirror of https://github.com/ArduPilot/ardupilot
AP_IOMCU: resync for 4.0 update
This commit is contained in:
parent
ee9d161196
commit
bdd7094d17
|
@ -334,6 +334,16 @@ void AP_IOMCU::read_status()
|
|||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_log_ms >= 1000U) {
|
||||
last_log_ms = now;
|
||||
if (AP_Logger::get_singleton()) {
|
||||
// @LoggerMessage: IOMC
|
||||
// @Description: IOMCU diagnostic information
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @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
|
||||
AP::logger().Write("IOMC", "TimeUS,Mem,TS,NPkt,Nerr,Nerr2,NDel", "QHIIIII",
|
||||
AP_HAL::micros64(),
|
||||
reg_status.freemem,
|
||||
|
@ -342,6 +352,7 @@ void AP_IOMCU::read_status()
|
|||
total_errors,
|
||||
reg_status.num_errors,
|
||||
num_delayed);
|
||||
}
|
||||
#if IOMCU_DEBUG_ENABLE
|
||||
static uint32_t last_io_print;
|
||||
if (now - last_io_print >= 5000) {
|
||||
|
|
|
@ -71,6 +71,11 @@ public:
|
|||
// get the name of the RC protocol
|
||||
const char *get_rc_protocol(void);
|
||||
|
||||
// get receiver RSSI
|
||||
int16_t get_RSSI(void) const {
|
||||
return rc_input.rssi;
|
||||
}
|
||||
|
||||
/*
|
||||
get servo rail voltage
|
||||
*/
|
||||
|
|
|
@ -304,6 +304,7 @@ void AP_IOMCU_FW::rcin_update()
|
|||
hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS);
|
||||
rc_last_input_ms = last_ms;
|
||||
rc_input.rc_protocol = (uint16_t)AP::RC().protocol_detected();
|
||||
rc_input.rssi = AP::RC().get_RSSI();
|
||||
} else if (last_ms - rc_last_input_ms > 200U) {
|
||||
rc_input.flags_rc_ok = false;
|
||||
}
|
||||
|
|
|
@ -123,6 +123,7 @@ struct page_rc_input {
|
|||
uint8_t flags_rc_ok:1;
|
||||
uint8_t rc_protocol;
|
||||
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
||||
int16_t rssi;
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
|
@ -28,7 +28,7 @@
|
|||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// usart3 is for SBUS input and output
|
||||
static const SerialConfig uart3_cfg = {
|
||||
static const SerialConfig sbus_cfg = {
|
||||
100000, // speed
|
||||
USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
|
||||
USART_CR2_STOP_1, // cr2, two stop bits
|
||||
|
@ -40,15 +40,19 @@ static const SerialConfig uart3_cfg = {
|
|||
// listen for parity errors on sd3 input
|
||||
static event_listener_t sd3_listener;
|
||||
|
||||
static uint8_t sd3_config;
|
||||
|
||||
void sbus_out_write(uint16_t *channels, uint8_t nchannels)
|
||||
{
|
||||
if (sd3_config == 0) {
|
||||
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 = {
|
||||
static const SerialConfig dsm_cfg = {
|
||||
115200, // speed
|
||||
0, // cr1
|
||||
0, // cr2
|
||||
|
@ -62,16 +66,17 @@ static const SerialConfig uart1_cfg = {
|
|||
*/
|
||||
void AP_IOMCU_FW::rcin_serial_init(void)
|
||||
{
|
||||
sdStart(&SD1, &uart1_cfg);
|
||||
sdStart(&SD3, &uart3_cfg);
|
||||
sdStart(&SD1, &dsm_cfg);
|
||||
sdStart(&SD3, &sbus_cfg);
|
||||
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
|
||||
&sd3_listener,
|
||||
EVENT_MASK(1),
|
||||
SD_PARITY_ERROR);
|
||||
// disable input for SBUS with pulses, we will use the UART for
|
||||
// SBUS.
|
||||
// SBUS and FPORT
|
||||
AP::RC().disable_for_pulses(AP_RCProtocol::SBUS);
|
||||
AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI);
|
||||
AP::RC().disable_for_pulses(AP_RCProtocol::FPORT);
|
||||
}
|
||||
|
||||
static struct {
|
||||
|
@ -79,6 +84,7 @@ static struct {
|
|||
uint32_t num_sbus_bytes;
|
||||
uint32_t num_sbus_errors;
|
||||
eventflags_t sbus_error;
|
||||
uint32_t last_good_ms;
|
||||
} rc_stats;
|
||||
|
||||
/*
|
||||
|
@ -88,13 +94,19 @@ void AP_IOMCU_FW::rcin_serial_update(void)
|
|||
{
|
||||
uint8_t b[16];
|
||||
uint32_t n;
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
// read from DSM port
|
||||
if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
|
||||
n = MIN(n, sizeof(b));
|
||||
// don't mix two 115200 uarts
|
||||
if (sd3_config == 0) {
|
||||
rc_stats.num_dsm_bytes += n;
|
||||
for (uint8_t i=0; i<n; i++) {
|
||||
AP::RC().process_byte(b[i], 115200);
|
||||
if (AP::RC().process_byte(b[i], 115200)) {
|
||||
rc_stats.last_good_ms = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
//BLUE_TOGGLE();
|
||||
}
|
||||
|
@ -102,17 +114,26 @@ void AP_IOMCU_FW::rcin_serial_update(void)
|
|||
// 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) {
|
||||
if (sd3_config == 0 && ((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++) {
|
||||
AP::RC().process_byte(b[i], 100000);
|
||||
if (AP::RC().process_byte(b[i], sd3_config==0?100000:115200)) {
|
||||
rc_stats.last_good_ms = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (now - rc_stats.last_good_ms > 2000) {
|
||||
rc_stats.last_good_ms = now;
|
||||
sd3_config ^= 1;
|
||||
sdStop(&SD3);
|
||||
sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
Loading…
Reference in New Issue