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:
parent
ec73a7072b
commit
285508d568
libraries/AP_IOMCU
@ -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) {
|
||||
|
@ -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((®_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((®_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
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user