5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

AP_IOMCU: added initial mixing support

supports aileron, elevator, throttle and rudder
This commit is contained in:
Andrew Tridgell 2018-10-31 13:10:51 +11:00
parent ec73a7072b
commit 285508d568
4 changed files with 92 additions and 7 deletions

View File

@ -23,13 +23,10 @@ extern const AP_HAL::HAL &hal;
enum ioevents {
IOEVENT_INIT=1,
IOEVENT_SEND_PWM_OUT,
IOEVENT_SET_DISARMED_PWM,
IOEVENT_SET_FAILSAFE_PWM,
IOEVENT_FORCE_SAFETY_OFF,
IOEVENT_FORCE_SAFETY_ON,
IOEVENT_SET_ONESHOT_ON,
IOEVENT_SET_RATES,
IOEVENT_GET_RCIN,
IOEVENT_ENABLE_SBUS,
IOEVENT_SET_HEATER_TARGET,
IOEVENT_SET_DEFAULT_RATE,
@ -226,7 +223,7 @@ void AP_IOMCU::thread_main(void)
// update safety pwm
if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
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;
}
}
@ -809,6 +806,7 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
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());
MIX_UPDATE(mixing.servo_reversed[i], ch->get_reversed());
}
// update RCMap
MIX_UPDATE(mixing.rc_channel[0], rcmap->roll());
@ -826,8 +824,18 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
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_reversed[i], ch->get_reverse());
// cope with reversible throttle
if (i == 2 && ch->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);
// and enable
MIX_UPDATE(mixing.enabled, 1);
if (changed) {

View File

@ -195,6 +195,15 @@ void AP_IOMCU_FW::update()
sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
}
// handle FMU failsafe
if (now - fmu_data_received_time > 10) {
// we are not getting input from the FMU. Fill in failsafe values
fill_failsafe_pwm();
chEvtSignal(thread_ctx, EVENT_MASK(IOEVENT_PWM));
// mark as done
fmu_data_received_time = now;
}
// run remaining functions at 1kHz
if (now != last_loop_ms) {
last_loop_ms = now;
@ -208,7 +217,6 @@ void AP_IOMCU_FW::update()
void AP_IOMCU_FW::pwm_out_update()
{
//TODO: PWM mixing
memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
hal.rcout->cork();
for (uint8_t i = 0; i < SERVO_COUNT; i++) {
@ -258,6 +266,14 @@ void AP_IOMCU_FW::rcin_update()
hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
}
// check for active override channel
if (mixing.enabled &&
mixing.rc_chan_override > 0 &&
mixing.rc_chan_override <= IOMCU_MAX_CHANNELS) {
override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1800);
} else {
override_active = false;
}
}
void AP_IOMCU_FW::process_io_packet()
@ -457,7 +473,12 @@ bool AP_IOMCU_FW::handle_code_write()
break;
}
break;
case PAGE_DIRECT_PWM: {
if (override_active) {
// no input when override is active
break;
}
/* copy channel data */
uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
@ -476,12 +497,25 @@ 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;
}
case PAGE_SAFETY_PWM: {
uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
memcpy((&reg_safety_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
case PAGE_FAILSAFE_PWM: {
uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
memcpy((&reg_failsafe_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
break;
}
default:
break;
}
@ -576,6 +610,23 @@ void AP_IOMCU_FW::rcout_mode_update(void)
}
}
/*
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();
#endif // HAL_BOARD_CHIBIOS

View File

@ -28,6 +28,12 @@ private:
void safety_update();
void rcout_mode_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;
struct PACKED {
/* default to RSSI ADC functionality */
@ -69,11 +75,21 @@ private:
uint16_t pwm[IOMCU_MAX_CHANNELS];
} reg_servo;
// PAGE_SERVO values
// 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;
@ -84,6 +100,9 @@ private:
// MIXER values
struct page_mixing mixing;
// true when override channel active
bool override_active;
// sbus rate handling
uint32_t sbus_last_ms;

View File

@ -49,7 +49,7 @@ enum iopage {
PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_DISARMED_PWM = 108,
PAGE_SAFETY_PWM = 108,
PAGE_MIXING = 200,
};
@ -148,16 +148,23 @@ struct PACKED page_mixing {
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];
// channel which when high forces mixer
int8_t rc_chan_override;
// is the throttle an angle input?
uint8_t throttle_is_angle;
// enabled needs to be 1 to enable mixing
uint8_t enabled;
uint8_t pad; // pad to even size
};