diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 4d1eefcfb9..4584f0f83b 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -309,7 +309,7 @@ uint16_t SRV_Channel::get_limit_pwm(Limit limit) const } // return true if function is for a multicopter motor -bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function) +bool SRV_Channel::is_motor(SRV_Channel::Function function) { return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) || (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12) || @@ -317,30 +317,30 @@ bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function) } // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous -bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function) +bool SRV_Channel::should_e_stop(SRV_Channel::Function function) { switch (function) { - case Aux_servo_function_t::k_heli_rsc: - case Aux_servo_function_t::k_heli_tail_rsc: - case Aux_servo_function_t::k_motor1: - case Aux_servo_function_t::k_motor2: - case Aux_servo_function_t::k_motor3: - case Aux_servo_function_t::k_motor4: - case Aux_servo_function_t::k_motor5: - case Aux_servo_function_t::k_motor6: - case Aux_servo_function_t::k_motor7: - case Aux_servo_function_t::k_motor8: - case Aux_servo_function_t::k_starter: - case Aux_servo_function_t::k_throttle: - case Aux_servo_function_t::k_throttleLeft: - case Aux_servo_function_t::k_throttleRight: - case Aux_servo_function_t::k_boost_throttle: - case Aux_servo_function_t::k_motor9: - case Aux_servo_function_t::k_motor10: - case Aux_servo_function_t::k_motor11: - case Aux_servo_function_t::k_motor12: - case Aux_servo_function_t::k_motor13 ... Aux_servo_function_t::k_motor32: - case Aux_servo_function_t::k_engine_run_enable: + case Function::k_heli_rsc: + case Function::k_heli_tail_rsc: + case Function::k_motor1: + case Function::k_motor2: + case Function::k_motor3: + case Function::k_motor4: + case Function::k_motor5: + case Function::k_motor6: + case Function::k_motor7: + case Function::k_motor8: + case Function::k_starter: + case Function::k_throttle: + case Function::k_throttleLeft: + case Function::k_throttleRight: + case Function::k_boost_throttle: + case Function::k_motor9: + case Function::k_motor10: + case Function::k_motor11: + case Function::k_motor12: + case Function::k_motor13 ... Function::k_motor32: + case Function::k_engine_run_enable: return true; default: return false; @@ -349,26 +349,25 @@ bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function) } // return true if function is for a control surface -bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function) +bool SRV_Channel::is_control_surface(SRV_Channel::Function function) { - switch (function) - { - case SRV_Channel::Aux_servo_function_t::k_flap: - case SRV_Channel::Aux_servo_function_t::k_flap_auto: - case SRV_Channel::Aux_servo_function_t::k_aileron: - case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft1: - case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft2: - case SRV_Channel::Aux_servo_function_t::k_dspoilerRight1: - case SRV_Channel::Aux_servo_function_t::k_dspoilerRight2: - case SRV_Channel::Aux_servo_function_t::k_elevator: - case SRV_Channel::Aux_servo_function_t::k_rudder: - case SRV_Channel::Aux_servo_function_t::k_flaperon_left: - case SRV_Channel::Aux_servo_function_t::k_flaperon_right: - case SRV_Channel::Aux_servo_function_t::k_elevon_left: - case SRV_Channel::Aux_servo_function_t::k_elevon_right: - case SRV_Channel::Aux_servo_function_t::k_vtail_left: - case SRV_Channel::Aux_servo_function_t::k_vtail_right: - case SRV_Channel::Aux_servo_function_t::k_airbrake: + switch (function) { + case Function::k_flap: + case Function::k_flap_auto: + case Function::k_aileron: + case Function::k_dspoilerLeft1: + case Function::k_dspoilerLeft2: + case Function::k_dspoilerRight1: + case Function::k_dspoilerRight2: + case Function::k_elevator: + case Function::k_rudder: + case Function::k_flaperon_left: + case Function::k_flaperon_right: + case Function::k_elevon_left: + case Function::k_elevon_right: + case Function::k_vtail_left: + case Function::k_vtail_right: + case Function::k_airbrake: return true; default: diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 886fb3b16d..eafb99431c 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -210,10 +210,10 @@ public: k_motor31 = 178, k_motor32 = 179, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) - } Aux_servo_function_t; + } Function; // check if a function is valid for indexing into functions - static bool valid_function(Aux_servo_function_t fn) { + static bool valid_function(Function fn) { return fn >= k_none && fn < k_nr_aux_servo_functions; } bool valid_function(void) const { @@ -268,24 +268,24 @@ public: } // return true if function is for a multicopter motor - static bool is_motor(SRV_Channel::Aux_servo_function_t function); + static bool is_motor(Function function); // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous - static bool should_e_stop(SRV_Channel::Aux_servo_function_t function); + static bool should_e_stop(Function function); // return true if function is for a control surface - static bool is_control_surface(SRV_Channel::Aux_servo_function_t function); + static bool is_control_surface(Function function); // return the function of a channel - SRV_Channel::Aux_servo_function_t get_function(void) const { - return (SRV_Channel::Aux_servo_function_t)function.get(); + SRV_Channel::Function get_function(void) const { + return (SRV_Channel::Function)function.get(); } // return the motor number of a channel, or -1 if not a motor int8_t get_motor_num(void) const; // set and save function for channel. Used in upgrade of parameters in plane - void function_set_and_save(SRV_Channel::Aux_servo_function_t f) { + void function_set_and_save(Function f) { function.set_and_save(int8_t(f)); } @@ -314,7 +314,7 @@ private: AP_Int16 servo_trim; // reversal, following convention that 1 means reversed, 0 means normal AP_Int8 reversed; - AP_Enum16 function; + AP_Enum16 function; // a pending output value as PWM uint16_t output_pwm; @@ -385,10 +385,10 @@ public: static const struct AP_Param::GroupInfo var_info[]; // set the default function for a channel - static void set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function); + static void set_default_function(uint8_t chan, SRV_Channel::Function function); // set output value for a function channel as a pwm value - static void set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value); + static void set_output_pwm(SRV_Channel::Function function, uint16_t value); // set output value for a specific function channel as a pwm value static void set_output_pwm_chan(uint8_t chan, uint16_t value); @@ -401,50 +401,50 @@ public: // set output value for a function channel as a scaled value. This // this should be followed by a call to calc_pwm() to output the pwm values - static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, float value); + static void set_output_scaled(SRV_Channel::Function function, float value); // get scaled output for the given function type. - static float get_output_scaled(SRV_Channel::Aux_servo_function_t function); + static float get_output_scaled(SRV_Channel::Function function); // get slew limited scaled output for the given function type - static float get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function); + static float get_slew_limited_output_scaled(SRV_Channel::Function function); // get pwm output for the first channel of the given function type. - static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value); + static bool get_output_pwm(SRV_Channel::Function function, uint16_t &value); // get normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max) // Value is taken from pwm value. Returns zero on error. - static float get_output_norm(SRV_Channel::Aux_servo_function_t function); + static float get_output_norm(SRV_Channel::Function function); // set normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max) for the given function - static void set_output_norm(SRV_Channel::Aux_servo_function_t function, float value); + static void set_output_norm(SRV_Channel::Function function, float value); // get output channel mask for a function - static uint32_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function); + static uint32_t get_output_channel_mask(SRV_Channel::Function function); // limit slew rate to given limit in percent per second - static void set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt); + static void set_slew_rate(SRV_Channel::Function function, float slew_rate, uint16_t range, float dt); // call output_ch() on all channels static void output_ch_all(void); // setup output ESC scaling based on a channels MIN/MAX - void set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function); + void set_esc_scaling_for(SRV_Channel::Function function); // return true when auto_trim enabled bool auto_trim_enabled(void) const { return auto_trim; } // adjust trim of a channel by a small increment - void adjust_trim(SRV_Channel::Aux_servo_function_t function, float v); + void adjust_trim(SRV_Channel::Function function, float v); // set MIN/MAX parameters for a function - static void set_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm); + static void set_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm); // set MIN/MAX parameter defaults for a function - static void set_output_min_max_defaults(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm); + static void set_output_min_max_defaults(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm); // Save MIN/MAX/REVERSED parameters for a function - static void save_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm); + static void save_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm); // save trims void save_trim(void); @@ -453,46 +453,46 @@ public: static void setup_failsafe_trim_all_non_motors(void); // set output for all channels matching the given function type, allow radio_trim to center servo - static void set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value); + static void set_output_pwm_trimmed(SRV_Channel::Function function, int16_t value); // set and save the trim for a function channel to the output value - static void set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t function); + static void set_trim_to_servo_out_for(SRV_Channel::Function function); // set the trim for a function channel to min of the channel honnoring reverse unless ignore_reversed is true - static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function, bool ignore_reversed = false); + static void set_trim_to_min_for(SRV_Channel::Function function, bool ignore_reversed = false); // set the trim for a function channel to given pwm - static void set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm); + static void set_trim_to_pwm_for(SRV_Channel::Function function, int16_t pwm); // set output to min value - static void set_output_to_min(SRV_Channel::Aux_servo_function_t function); + static void set_output_to_min(SRV_Channel::Function function); // set output to max value - static void set_output_to_max(SRV_Channel::Aux_servo_function_t function); + static void set_output_to_max(SRV_Channel::Function function); // set output to trim value - static void set_output_to_trim(SRV_Channel::Aux_servo_function_t function); + static void set_output_to_trim(SRV_Channel::Function function); // copy radio_in to servo out - static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false); + static void copy_radio_in_out(SRV_Channel::Function function, bool do_input_output=false); // copy radio_in to servo_out by channel mask static void copy_radio_in_out_mask(uint32_t mask); // setup failsafe for an auxiliary channel function, by pwm - static void set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm); + static void set_failsafe_pwm(SRV_Channel::Function function, uint16_t pwm); // setup failsafe for an auxiliary channel function - static void set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit); + static void set_failsafe_limit(SRV_Channel::Function function, SRV_Channel::Limit limit); // set servo to a Limit - static void set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit); + static void set_output_limit(SRV_Channel::Function function, SRV_Channel::Limit limit); // return true if a function is assigned to a channel - static bool function_assigned(SRV_Channel::Aux_servo_function_t function); + static bool function_assigned(SRV_Channel::Function function); // set a servo_out value, and angle range, then calc_pwm - static void move_servo(SRV_Channel::Aux_servo_function_t function, + static void move_servo(SRV_Channel::Function function, int16_t value, int16_t angle_min, int16_t angle_max); // assign and enable auxiliary channels @@ -502,28 +502,28 @@ public: static void enable_by_mask(uint32_t mask); // return the current function for a channel - static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel); + static SRV_Channel::Function channel_function(uint8_t channel); // refresh aux servo to function mapping static void update_aux_servo_function(void); // set default channel for an auxiliary function - static bool set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel); + static bool set_aux_channel_default(SRV_Channel::Function function, uint8_t channel); // find first channel that a function is assigned to - static bool find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan); + static bool find_channel(SRV_Channel::Function function, uint8_t &chan); // find first channel that a function is assigned to, returning SRV_Channel object - static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function); + static SRV_Channel *get_channel_for(SRV_Channel::Function function); // call set_angle() on matching channels - static void set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle); + static void set_angle(SRV_Channel::Function function, uint16_t angle); // call set_range() on matching channels - static void set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range); + static void set_range(SRV_Channel::Function function, uint16_t range); // set output refresh frequency on a servo function - static void set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency); + static void set_rc_frequency(SRV_Channel::Function function, uint16_t frequency); // control pass-thru of channels void disable_passthrough(bool disable) { @@ -531,7 +531,7 @@ public: } // constrain to output min/max for function - static void constrain_pwm(SRV_Channel::Aux_servo_function_t function); + static void constrain_pwm(SRV_Channel::Function function); // calculate PWM for all channels static void calc_pwm(void); @@ -555,14 +555,14 @@ public: static void upgrade_parameters(void); // given a zero-based motor channel, return the k_motor function for that channel - static SRV_Channel::Aux_servo_function_t get_motor_function(uint8_t channel) { + static SRV_Channel::Function get_motor_function(uint8_t channel) { if (channel < 8) { - return SRV_Channel::Aux_servo_function_t(SRV_Channel::k_motor1+channel); + return SRV_Channel::Function(SRV_Channel::k_motor1+channel); } if (channel < 12) { - return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8))); + return SRV_Channel::Function((SRV_Channel::k_motor9+(channel-8))); } - return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor13+(channel-12))); + return SRV_Channel::Function((SRV_Channel::k_motor13+(channel-12))); } void cork(); @@ -701,8 +701,8 @@ private: // linked list for slew rate handling struct slew_list { - slew_list(SRV_Channel::Aux_servo_function_t _func) : func(_func) {}; - const SRV_Channel::Aux_servo_function_t func; + slew_list(SRV_Channel::Function _func) : func(_func) {}; + const SRV_Channel::Function func; float last_scaled_output; float max_change; slew_list * next; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index a08be42da3..336f4db573 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -120,7 +120,7 @@ void SRV_Channels::output_ch_all(void) /* return the current function for a channel */ -SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel) +SRV_Channel::Function SRV_Channels::channel_function(uint8_t channel) { if (channel < NUM_SERVO_CHANNELS) { return channels[channel].function; @@ -336,7 +336,7 @@ void SRV_Channels::enable_by_mask(uint32_t mask) /* set radio_out for all channels matching the given function type */ -void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value) +void SRV_Channels::set_output_pwm(SRV_Channel::Function function, uint16_t value) { if (!function_assigned(function)) { return; @@ -355,7 +355,7 @@ void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, ui reverses pwm output based on channel reversed property */ void -SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value) +SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Function function, int16_t value) { if (!function_assigned(function)) { return; @@ -379,7 +379,7 @@ SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, the given function type */ void -SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t function) +SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Function function) { if (!function_assigned(function)) { return; @@ -396,7 +396,7 @@ SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t functi copy radio_in to radio_out for a given function */ void -SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output) +SRV_Channels::copy_radio_in_out(SRV_Channel::Function function, bool do_input_output) { if (!function_assigned(function)) { return; @@ -438,7 +438,7 @@ SRV_Channels::copy_radio_in_out_mask(uint32_t mask) setup failsafe value for an auxiliary function type to a Limit */ void -SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm) +SRV_Channels::set_failsafe_pwm(SRV_Channel::Function function, uint16_t pwm) { if (!function_assigned(function)) { return; @@ -455,7 +455,7 @@ SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint1 setup failsafe value for an auxiliary function type to a Limit */ void -SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit) +SRV_Channels::set_failsafe_limit(SRV_Channel::Function function, SRV_Channel::Limit limit) { if (!function_assigned(function)) { return; @@ -473,7 +473,7 @@ SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV set radio output value for an auxiliary function type to a Limit */ void -SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit) +SRV_Channels::set_output_limit(SRV_Channel::Function function, SRV_Channel::Limit limit) { if (!function_assigned(function)) { return; @@ -501,7 +501,7 @@ SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_C return true if a particular function is assigned to at least one RC channel */ bool -SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function) +SRV_Channels::function_assigned(SRV_Channel::Function function) { if (!initialised) { update_aux_servo_function(); @@ -514,7 +514,7 @@ SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function) value. This is used to move a AP_Mount servo */ void -SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function, +SRV_Channels::move_servo(SRV_Channel::Function function, int16_t value, int16_t angle_min, int16_t angle_max) { if (!function_assigned(function)) { @@ -538,7 +538,7 @@ SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function, /* set the default channel an auxiliary output function should be on */ -bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel) +bool SRV_Channels::set_aux_channel_default(SRV_Channel::Function function, uint8_t channel) { if (function_assigned(function)) { // already assigned @@ -565,7 +565,7 @@ bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t fun } // find first channel that a function is assigned to -bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan) +bool SRV_Channels::find_channel(SRV_Channel::Function function, uint8_t &chan) { // Must have populated channel masks if (!initialised) { @@ -591,7 +591,7 @@ bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint /* get a pointer to first auxiliary channel for a channel function */ -SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function) +SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Function function) { uint8_t chan; if (!find_channel(function, chan)) { @@ -600,7 +600,7 @@ SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t fun return &channels[chan]; } -void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, float value) +void SRV_Channels::set_output_scaled(SRV_Channel::Function function, float value) { if (SRV_Channel::valid_function(function)) { functions[function].output_scaled = value; @@ -608,7 +608,7 @@ void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, } } -float SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function) +float SRV_Channels::get_output_scaled(SRV_Channel::Function function) { if (SRV_Channel::valid_function(function)) { return functions[function].output_scaled; @@ -617,7 +617,7 @@ float SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function } // get slew limited scaled output for the given function type -float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function) +float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Function function) { if (!SRV_Channel::valid_function(function)) { return 0.0; @@ -638,7 +638,7 @@ float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Aux_servo_functi /* get mask of output channels for a function */ -uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t function) +uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Function function) { if (!initialised) { update_aux_servo_function(); @@ -651,7 +651,7 @@ uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t // set the trim for a function channel to given pwm -void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm) +void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Function function, int16_t pwm) { for (uint8_t i=0; i