Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: limit rate to IOMCU to 500Hz

Browse Source 
when running with very high loop rates we can saturate the IO link
This commit is contained in:
Andrew Tridgell 2018-01-16 21:21:59 +11:00
parent 880a84294e
commit 93fa74518a
2 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -148,9 +148,6 @@ void AP_IOMCU::thread_main(void)
trigger_event(IOEVENT_INIT);
// enable sbus (until we have BRD_SBUS_OUT available in ChibiOS)
enable_sbus_out(150);
while (true) {
eventmask_t mask = chEvtWaitAnyTimeout(~0, MS2ST(10));
@ -257,7 +254,16 @@ void AP_IOMCU::thread_main(void)
void AP_IOMCU::send_servo_out()
{
if (pwm_out.num_channels > 0) {
write_registers(PAGE_DIRECT_PWM, 0, pwm_out.num_channels, pwm_out.pwm);
uint8_t n = pwm_out.num_channels;
if (rate.sbus_rate_hz == 0) {
n = MIN(n, 8);
}
uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) {
// don't send data at more than 500Hz
write_registers(PAGE_DIRECT_PWM, 0, n, pwm_out.pwm);
last_servo_out_us = now;
}
}
}
@ -266,8 +272,8 @@ void AP_IOMCU::send_servo_out()
*/
void AP_IOMCU::read_rc_input()
{
// read a min of 9 channels and max of max_channels
uint8_t n = MIN(MAX(9, rc_input.count), max_channels);
// read a min of 9 channels and max of 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);
if (rc_input.flags_rc_ok) {
rc_input.last_input_us = AP_HAL::micros();
@ -432,7 +438,7 @@ bool AP_IOMCU::modify_register(uint8_t page, uint8_t offset, uint16_t clearbits,
void AP_IOMCU::write_channel(uint8_t chan, uint16_t pwm)
{
if (chan >= max_channels) {
if (chan >= IOMCU_MAX_CHANNELS) {
return;
}
if (chan >= pwm_out.num_channels) {
@ -532,7 +538,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 != rc_input.last_input_us) {
num_channels = MIN(MIN(rc_input.count, max_channels), max_chan);
num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_CHANNELS), max_chan);
memcpy(channels, rc_input.pwm, num_channels*2);
last_frame_us = rc_input.last_input_us;
return true;

14
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -11,6 +11,8 @@
#include "ch.h"
#define IOMCU_MAX_CHANNELS 16
class AP_IOMCU {
public:
AP_IOMCU(AP_HAL::UARTDriver &uart);
@ -113,8 +115,6 @@ private:
void discard_input(void);
void event_failed(uint8_t event);
static const uint8_t max_channels = 16;
// PAGE_STATUS values
struct PACKED {
uint16_t freemem;
@ -159,7 +159,7 @@ private:
uint16_t data;
uint16_t frame_count;
uint16_t lost_frame_count;
uint16_t pwm[max_channels];
uint16_t pwm[IOMCU_MAX_CHANNELS];
uint16_t last_frame_count;
uint32_t last_input_us;
} rc_input;
@ -167,12 +167,12 @@ private:
// output pwm values
struct {
uint8_t num_channels;
uint16_t pwm[max_channels];
uint16_t pwm[IOMCU_MAX_CHANNELS];
} pwm_out;
// read back pwm values
struct {
uint16_t pwm[max_channels];
uint16_t pwm[IOMCU_MAX_CHANNELS];
} pwm_in;
// output rates
@ -185,7 +185,9 @@ private:
// IMU heater duty cycle
uint8_t heater_duty_cycle;
uint32_t last_servo_out_us;
bool corked;
};