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

SRV_Channel: make function an Enum16

Browse Source
This commit is contained in:
Peter Barker 2021-11-29 17:56:33 +11:00 committed by Andrew Tridgell
parent b1cff2d3f5
commit 7c126b1e7b
3 changed files with 31 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SRV_Channel/SRV_Channel.h
View File

@ -270,7 +270,7 @@ private:
AP_Int16 servo_trim; AP_Int16 servo_trim;
// reversal, following convention that 1 means reversed, 0 means normal // reversal, following convention that 1 means reversed, 0 means normal
AP_Int8 reversed; AP_Int8 reversed;
AP_Int16 function; AP_Enum16<Aux_servo_function_t> function;
// a pending output value as PWM // a pending output value as PWM
uint16_t output_pwm; uint16_t output_pwm;

53
libraries/SRV_Channel/SRV_Channel_aux.cpp
View File

@ -34,13 +34,13 @@ void SRV_Channel::output_ch(void)
int8_t passthrough_from = -1; int8_t passthrough_from = -1;
// take care of special function cases // take care of special function cases
switch(function) switch(function.get())
{ {
case k_manual: // manual case k_manual: // manual
passthrough_from = ch_num; passthrough_from = ch_num;
break; break;
case k_rcin1 ... k_rcin16: // rc pass-thru case k_rcin1 ... k_rcin16: // rc pass-thru
passthrough_from = int8_t(function - k_rcin1); passthrough_from = int8_t((int16_t)function - k_rcin1);
break; break;
} }
if (passthrough_from != -1) { if (passthrough_from != -1) {
@ -87,7 +87,7 @@ void SRV_Channels::output_ch_all(void)
SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel) SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel)
{ {
if (channel < NUM_SERVO_CHANNELS) { if (channel < NUM_SERVO_CHANNELS) {
return (SRV_Channel::Aux_servo_function_t)channels[channel].function.get(); return channels[channel].function;
} }
return SRV_Channel::k_none; return SRV_Channel::k_none;
} }
@ -100,7 +100,7 @@ void SRV_Channel::aux_servo_function_setup(void)
if (type_setup) { if (type_setup) {
return; return;
} }
switch (function) { switch (function.get()) {
case k_flap: case k_flap:
case k_flap_auto: case k_flap_auto:
case k_egg_drop: case k_egg_drop:
@ -181,11 +181,14 @@ void SRV_Channels::update_aux_servo_function(void)
// set auxiliary ranges // set auxiliary ranges
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if ((uint16_t)channels[i].function.get() < SRV_Channel::k_nr_aux_servo_functions) { const uint16_t function_num = channels[i].function.get();
channels[i].aux_servo_function_setup(); if (function_num >= SRV_Channel::k_nr_aux_servo_functions) {
function_mask.set((uint16_t)channels[i].function.get()); // invalid function number
functions[channels[i].function.get()].channel_mask |= 1U<<i; continue;
} }
channels[i].aux_servo_function_setup();
function_mask.set(function_num);
functions[function_num].channel_mask |= 1U<<i;
} }
initialised = true; initialised = true;
} }
@ -204,19 +207,19 @@ void SRV_Channels::enable_aux_servos()
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i]; SRV_Channel &c = channels[i];
// see if it is a valid function // see if it is a valid function
if ((uint16_t)c.function.get() < SRV_Channel::k_nr_aux_servo_functions) { if (c.function < SRV_Channel::k_nr_aux_servo_functions) {
hal.rcout->enable_ch(c.ch_num); hal.rcout->enable_ch(c.ch_num);
} }
// output some servo functions before we fiddle with the // output some servo functions before we fiddle with the
// parameter values: // parameter values:
if (c.function.get() == SRV_Channel::k_min) { if (c.function == SRV_Channel::k_min) {
c.set_output_pwm(c.servo_min); c.set_output_pwm(c.servo_min);
c.output_ch(); c.output_ch();
} else if (c.function.get() == SRV_Channel::k_trim) { } else if (c.function == SRV_Channel::k_trim) {
c.set_output_pwm(c.servo_trim); c.set_output_pwm(c.servo_trim);
c.output_ch(); c.output_ch();
} else if (c.function.get() == SRV_Channel::k_max) { } else if (c.function == SRV_Channel::k_max) {
c.set_output_pwm(c.servo_max); c.set_output_pwm(c.servo_max);
c.output_ch(); c.output_ch();
} }
@ -277,7 +280,7 @@ void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, ui
return; return;
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) { if (channels[i].function == function) {
channels[i].set_output_pwm(value); channels[i].set_output_pwm(value);
channels[i].output_ch(); channels[i].output_ch();
} }
@ -296,7 +299,7 @@ SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function,
return; return;
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) { if (channels[i].function == function) {
int16_t value2; int16_t value2;
if (channels[i].get_reversed()) { if (channels[i].get_reversed()) {
value2 = 1500 - value + channels[i].get_trim(); value2 = 1500 - value + channels[i].get_trim();
@ -320,7 +323,7 @@ SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t functi
return; return;
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) { if (channels[i].function == function) {
channels[i].servo_trim.set_and_save_ifchanged(channels[i].get_output_pwm()); channels[i].servo_trim.set_and_save_ifchanged(channels[i].get_output_pwm());
} }
} }
@ -336,7 +339,7 @@ SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool
return; return;
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) { if (channels[i].function == function) {
RC_Channel *c = rc().channel(channels[i].ch_num); RC_Channel *c = rc().channel(channels[i].ch_num);
if (c == nullptr) { if (c == nullptr) {
continue; continue;
@ -378,7 +381,7 @@ SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint1
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel &c = channels[i]; const SRV_Channel &c = channels[i];
if (c.function.get() == function) { if (c.function == function) {
hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm); hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
} }
} }
@ -395,7 +398,7 @@ SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel &c = channels[i]; const SRV_Channel &c = channels[i];
if (c.function.get() == function) { if (c.function == function) {
uint16_t pwm = c.get_limit_pwm(limit); uint16_t pwm = c.get_limit_pwm(limit);
hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm); hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
} }
@ -413,10 +416,10 @@ SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_C
} }
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i]; SRV_Channel &c = channels[i];
if (c.function.get() == function) { if (c.function == function) {
uint16_t pwm = c.get_limit_pwm(limit); uint16_t pwm = c.get_limit_pwm(limit);
c.set_output_pwm(pwm); c.set_output_pwm(pwm);
if (c.function.get() == SRV_Channel::k_manual) { if (c.function == SRV_Channel::k_manual) {
RC_Channel *cin = rc().channel(c.ch_num); RC_Channel *cin = rc().channel(c.ch_num);
if (cin != nullptr) { if (cin != nullptr) {
// in order for output_ch() to work for k_manual we // in order for output_ch() to work for k_manual we
@ -458,7 +461,7 @@ SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function,
v = constrain_float(v, 0.0f, 1.0f); v = constrain_float(v, 0.0f, 1.0f);
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i]; SRV_Channel &c = channels[i];
if (c.function.get() == function) { if (c.function == function) {
float v2 = c.get_reversed()? (1-v) : v; float v2 = c.get_reversed()? (1-v) : v;
uint16_t pwm = c.servo_min + v2 * (c.servo_max - c.servo_min); uint16_t pwm = c.servo_min + v2 * (c.servo_max - c.servo_min);
c.set_output_pwm(pwm); c.set_output_pwm(pwm);
@ -481,7 +484,7 @@ bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t fun
} }
hal.console->printf("Channel %u already assigned function %u\n", hal.console->printf("Channel %u already assigned function %u\n",
(unsigned)(channel + 1), (unsigned)(channel + 1),
(unsigned)channels[channel].function); (unsigned)channels[channel].function.get());
return false; return false;
} }
channels[channel].type_setup = false; channels[channel].type_setup = false;
@ -579,8 +582,8 @@ void SRV_Channels::set_trim_to_min_for(SRV_Channel::Aux_servo_function_t functio
void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function) void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function)
{ {
if (chan < NUM_SERVO_CHANNELS) { if (chan < NUM_SERVO_CHANNELS) {
int8_t old = channels[chan].function; const SRV_Channel::Aux_servo_function_t old = channels[chan].function;
channels[chan].function.set_default((uint16_t)function); channels[chan].function.set_default(function);
if (old != channels[chan].function && channels[chan].function == function) { if (old != channels[chan].function && channels[chan].function == function) {
function_mask.set((uint16_t)function); function_mask.set((uint16_t)function);
} }
@ -606,7 +609,7 @@ void SRV_Channels::adjust_trim(SRV_Channel::Aux_servo_function_t function, float
} }
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) { for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i]; SRV_Channel &c = channels[i];
if (function != (SRV_Channel::Aux_servo_function_t)(c.function.get())) { if (function != c.function) {
continue; continue;
} }
float change = c.reversed?-v:v; float change = c.reversed?-v:v;

3
libraries/SRV_Channel/SRV_Channels.cpp
View File

@ -317,7 +317,8 @@ void SRV_Channels::calc_pwm(void)
channels[i].set_override(true); channels[i].set_override(true);
override_counter[i]--; override_counter[i]--;
} }
channels[i].calc_pwm(functions[channels[i].function].output_scaled); const uint16_t function_num = channels[i].function.get();
channels[i].calc_pwm(functions[function_num].output_scaled);
} }
} }