From 572df8b8599288d225b9012435ca79edf2cce18a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Jul 2017 12:23:09 +0900 Subject: [PATCH] SRV_Channel: minor formatting fixes no functional change --- libraries/SRV_Channel/SRV_Channel.cpp | 5 ++--- libraries/SRV_Channel/SRV_Channel.h | 20 +++++++++---------- libraries/SRV_Channel/SRV_Channel_aux.cpp | 24 ++++++++--------------- libraries/SRV_Channel/SRV_Channels.cpp | 6 +++--- 4 files changed, 23 insertions(+), 32 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index d816218d3d..af062ee390 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -54,7 +54,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Increment: 1 // @User: Standard AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500), - + // @Param: REVERSED // @DisplayName: Servo reverse // @Description: Reverse servo operation. Set to 0 for normal operation. Set to 1 to reverse this output channel. @@ -68,7 +68,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Values: 0:Disabled,1:RCPassThru,2:Flap,3:Flap_auto,4:Aileron,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release,12:mount2_pan,13:mount2_tilt,14:mount2_roll,15:mount2_open,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,19:Elevator,21:Rudder,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,41:MotorTilt,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,68:Choke,69:Starter,70:Throttle,71:TrackerYaw,72:TrackerPitch,73:ThrottleLeft,74:ThrottleRight,75:tiltMotorLeft,76:tiltMotorRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12 // @User: Standard AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), - + AP_GROUPEND }; @@ -80,7 +80,6 @@ SRV_Channel::SRV_Channel(void) have_pwm_mask = ~uint16_t(0); } - // convert a 0..range_max to a pwm uint16_t SRV_Channel::pwm_from_range(int16_t scaled_value) const { diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index bc87b290fd..e0deb34c11 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -182,7 +182,7 @@ private: // set_range() or set_angle() has been called bool type_setup:1; - + // the hal channel number uint8_t ch_num; @@ -209,7 +209,7 @@ private: // get normalised output from -1 to 1 float get_output_norm(void); - + // a bitmask type wide enough for NUM_SERVO_CHANNELS typedef uint16_t servo_mask_t; @@ -224,7 +224,7 @@ private: class SRV_Channels { public: friend class SRV_Channel; - + // constructor SRV_Channels(void); @@ -241,7 +241,7 @@ public: // set output value for a specific function channel as a pwm value static void set_output_pwm_chan(uint8_t chan, uint16_t value); - + // set output value for a function channel as a scaled value. This // calls calc_pwm() to also set the pwm value static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value); @@ -290,7 +290,7 @@ public: // 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); - + // set the trim for a function channel to min of the channel static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function); @@ -333,7 +333,7 @@ public: // enable channels by mask static void enable_by_mask(uint16_t mask); - + // return the current function for a channel static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel); @@ -357,7 +357,7 @@ public: // set output refresh frequency on a servo function static void set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency); - + // control pass-thru of channels void disable_passthrough(bool disable) { disabled_passthrough = disable; @@ -365,7 +365,7 @@ public: // constrain to output min/max for function static void constrain_pwm(SRV_Channel::Aux_servo_function_t function); - + // calculate PWM for all channels static void calc_pwm(void); @@ -376,7 +376,7 @@ public: // upgrade RC* parameters into SERVO* parameters static bool upgrade_parameters(const uint8_t old_keys[14], uint16_t aux_channel_mask, RCMapper *rcmap); static void upgrade_motors_servo(uint8_t ap_motors_key, uint8_t ap_motors_idx, uint8_t new_channel); - + private: struct { bool k_throttle_reversible:1; @@ -388,7 +388,7 @@ private: static Bitmask function_mask; static bool initialised; - + // this static arrangement is to avoid having static objects in AP_Param tables static SRV_Channel *channels; static SRV_Channels *instance; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 0957dd7cc3..bf724011fe 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -137,7 +137,7 @@ void SRV_Channels::update_aux_servo_function(void) for (uint8_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) { functions[i].channel_mask = 0; } - + // set auxiliary ranges for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { channels[i].aux_servo_function_setup(); @@ -147,12 +147,11 @@ void SRV_Channels::update_aux_servo_function(void) initialised = true; } - /// Should be called after the the servo functions have been initialized void SRV_Channels::enable_aux_servos() { hal.rcout->set_default_rate(uint16_t(instance->default_rate.get())); - + update_aux_servo_function(); // enable all channels that are set to a valid function. This @@ -446,7 +445,6 @@ int16_t SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t functi return 0; } - // 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) { @@ -481,7 +479,6 @@ void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_fun } } - void SRV_Channels::set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function) { uint8_t chan; @@ -520,7 +517,6 @@ void SRV_Channels::adjust_trim(SRV_Channel::Aux_servo_function_t function, float } } - // get pwm output for the first channel of the given function type. bool SRV_Channels::get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value) { @@ -650,7 +646,7 @@ bool SRV_Channels::upgrade_parameters(const uint8_t rc_keys[14], uint16_t aux_ch // upgrade already done return false; } - + // old system had 14 RC channels for (uint8_t i=0; i<14; i++) { uint8_t k = rc_keys[i]; @@ -679,7 +675,7 @@ bool SRV_Channels::upgrade_parameters(const uint8_t rc_keys[14], uint16_t aux_ch { 1, &srv_chan.function, nullptr, AP_PARAM_INT8, FLAG_AUX_ONLY }, }; bool is_aux = aux_channel_mask & (1U<configured_in_storage()) { // not configured yet in new eeprom if (m.type == AP_PARAM_INT16) { @@ -741,15 +737,12 @@ bool SRV_Channels::upgrade_parameters(const uint8_t rc_keys[14], uint16_t aux_ch } } - // mark the upgrade as having been done channels[15].function.set_and_save(channels[15].function.get()); return true; } - - /* Upgrade servo MIN/MAX/TRIM/REVERSE parameters for a single AP_Motors RC_Channel servo from previous firmwares, setting the equivalent @@ -773,7 +766,7 @@ void SRV_Channels::upgrade_motors_servo(uint8_t ap_motors_key, uint8_t ap_motors { 2, &srv_chan.servo_max, AP_PARAM_INT16, FLAG_NONE }, { 3, &srv_chan.reversed, AP_PARAM_INT8, FLAG_IS_REVERSE }, }; - + for (uint8_t j=0; j