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: allow up to 16 channels of servo data to be sent to the iomcu

Browse Source
This commit is contained in:
Andy Piper 2024-04-27 08:44:31 +01:00 committed by Andrew Tridgell
parent 29cbd8da15
commit 41fd7c726e
5 changed files with 17 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -791,7 +791,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) void AP_IOMCU::write_channel(uint8_t chan, uint16_t pwm)
{ {
if (chan >= IOMCU_MAX_CHANNELS) { if (chan >= IOMCU_MAX_RC_CHANNELS) { // could be SBUS out
return; return;
} }
if (chan >= pwm_out.num_channels) { if (chan >= pwm_out.num_channels) {
@ -1112,7 +1112,7 @@ bool AP_IOMCU::check_crc(void)
void AP_IOMCU::set_failsafe_pwm(uint16_t chmask, uint16_t period_us) void AP_IOMCU::set_failsafe_pwm(uint16_t chmask, uint16_t period_us)
{ {
bool changed = false; bool changed = false;
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) { for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
if (chmask & (1U<<i)) { if (chmask & (1U<<i)) {
if (pwm_out.failsafe_pwm[i] != period_us) { if (pwm_out.failsafe_pwm[i] != period_us) {
pwm_out.failsafe_pwm[i] = period_us; pwm_out.failsafe_pwm[i] = period_us;
@ -1191,7 +1191,7 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
#define MIX_UPDATE(a,b) do { if ((a) != (b)) { a = b; changed = true; }} while (0) #define MIX_UPDATE(a,b) do { if ((a) != (b)) { a = b; changed = true; }} while (0)
// update mixing structure, checking for changes // update mixing structure, checking for changes
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) { for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
const SRV_Channel *c = SRV_Channels::srv_channel(i); const SRV_Channel *c = SRV_Channels::srv_channel(i);
if (!c) { if (!c) {
continue; continue;

6
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -243,9 +243,9 @@ private:
// output pwm values // output pwm values
struct { struct {
uint8_t num_channels; uint8_t num_channels;
uint16_t pwm[IOMCU_MAX_CHANNELS]; uint16_t pwm[IOMCU_MAX_RC_CHANNELS];
uint16_t safety_mask; uint16_t safety_mask;
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS]; uint16_t failsafe_pwm[IOMCU_MAX_RC_CHANNELS];
uint8_t failsafe_pwm_set; uint8_t failsafe_pwm_set;
uint8_t failsafe_pwm_sent; uint8_t failsafe_pwm_sent;
uint16_t channel_mask; uint16_t channel_mask;
@ -253,7 +253,7 @@ private:
// read back pwm values // read back pwm values
struct { struct {
uint16_t pwm[IOMCU_MAX_CHANNELS]; uint16_t pwm[IOMCU_MAX_RC_CHANNELS];
} pwm_in; } pwm_in;
// output rates // output rates

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

@ -1216,7 +1216,7 @@ void AP_IOMCU_FW::rcout_config_update(void)
*/ */
void AP_IOMCU_FW::fill_failsafe_pwm(void) void AP_IOMCU_FW::fill_failsafe_pwm(void)
{ {
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) { for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
if (reg_status.flag_safety_off) { if (reg_status.flag_safety_off) {
reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i]; reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i];
} else { } else {

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

@ -9,7 +9,9 @@
// 22 is enough for the rc_input page in one transfer // 22 is enough for the rc_input page in one transfer
#define PKT_MAX_REGS 22 #define PKT_MAX_REGS 22
// The number of channels that can be propagated - due to SBUS_OUT is higher than the physical channels
#define IOMCU_MAX_RC_CHANNELS 16 #define IOMCU_MAX_RC_CHANNELS 16
// The actual number of output channels
#define IOMCU_MAX_CHANNELS 8 #define IOMCU_MAX_CHANNELS 8
#define IOMCU_MAX_TELEM_CHANNELS 4 #define IOMCU_MAX_TELEM_CHANNELS 4
@ -155,17 +157,17 @@ struct page_rc_input {
data for mixing on FMU failsafe data for mixing on FMU failsafe
*/ */
struct page_mixing { struct page_mixing {
uint16_t servo_min[IOMCU_MAX_CHANNELS]; uint16_t servo_min[IOMCU_MAX_RC_CHANNELS];
uint16_t servo_max[IOMCU_MAX_CHANNELS]; uint16_t servo_max[IOMCU_MAX_RC_CHANNELS];
uint16_t servo_trim[IOMCU_MAX_CHANNELS]; uint16_t servo_trim[IOMCU_MAX_RC_CHANNELS];
uint8_t servo_function[IOMCU_MAX_CHANNELS]; uint8_t servo_function[IOMCU_MAX_RC_CHANNELS];
uint8_t servo_reversed[IOMCU_MAX_CHANNELS]; uint8_t servo_reversed[IOMCU_MAX_RC_CHANNELS];
// RC input arrays are in AETR order // RC input arrays are in AETR order
uint16_t rc_min[4]; uint16_t rc_min[4];
uint16_t rc_max[4]; uint16_t rc_max[4];
uint16_t rc_trim[4]; uint16_t rc_trim[4];
uint8_t rc_reversed[IOMCU_MAX_CHANNELS]; uint8_t rc_reversed[IOMCU_MAX_RC_CHANNELS];
uint8_t rc_channel[4]; uint8_t rc_channel[4];
// gain for elevon and vtail mixing, x1000 // gain for elevon and vtail mixing, x1000

4
libraries/AP_IOMCU/iofirmware/mixer.cpp
View File

@ -130,7 +130,7 @@ void AP_IOMCU_FW::run_mixer(void)
// get RC input angles // get RC input angles
if (rc_input.flags_rc_ok) { if (rc_input.flags_rc_ok) {
for (uint8_t i=0;i<4; i++) { for (uint8_t i=0;i<4; i++) {
if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_CHANNELS) { if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_RC_CHANNELS) {
uint8_t chan = mixing.rc_channel[i]-1; uint8_t chan = mixing.rc_channel[i]-1;
if (i == 2 && !mixing.throttle_is_angle) { if (i == 2 && !mixing.throttle_is_angle) {
rcin[i] = mix_input_range(i, rc_input.pwm[chan]); rcin[i] = mix_input_range(i, rc_input.pwm[chan]);
@ -141,7 +141,7 @@ void AP_IOMCU_FW::run_mixer(void)
} }
} }
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) { for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i]; SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i];
uint16_t &pwm = reg_direct_pwm.pwm[i]; uint16_t &pwm = reg_direct_pwm.pwm[i];