diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 656b655bc3..d8f834ea16 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -12,7 +12,7 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] = { // @Param: FUNCTION // @DisplayName: Servo out function // @Description: Setting this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function - // @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:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable + // @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:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC // @User: Standard AP_GROUPINFO("FUNCTION", 1, RC_Channel_aux, function, 0), @@ -20,7 +20,7 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] = { }; RC_Channel_aux *RC_Channel_aux::_aux_channels[RC_AUX_MAX_CHANNELS]; -uint32_t RC_Channel_aux::_function_mask; +uint64_t RC_Channel_aux::_function_mask; /// map a function to a servo channel and output it void @@ -78,6 +78,44 @@ RC_Channel_aux::Aux_servo_function_t RC_Channel_aux::channel_function(uint8_t ch return RC_Channel_aux::k_none; } +/* + setup a channels aux servo function +*/ +void RC_Channel_aux::aux_servo_function_setup(void) +{ + switch (function) { + case RC_Channel_aux::k_flap: + case RC_Channel_aux::k_flap_auto: + case RC_Channel_aux::k_egg_drop: + set_range_out(0,100); + break; + case RC_Channel_aux::k_heli_rsc: + case RC_Channel_aux::k_heli_tail_rsc: + set_range_out(0,1000); + break; + case RC_Channel_aux::k_aileron_with_input: + case RC_Channel_aux::k_elevator_with_input: + set_angle(4500); + break; + case RC_Channel_aux::k_aileron: + case RC_Channel_aux::k_elevator: + case RC_Channel_aux::k_dspoiler1: + case RC_Channel_aux::k_dspoiler2: + case RC_Channel_aux::k_rudder: + case RC_Channel_aux::k_steering: + case RC_Channel_aux::k_flaperon1: + case RC_Channel_aux::k_flaperon2: + set_angle_out(4500); + break; + default: + break; + } + + if (function < k_nr_aux_servo_functions) { + _function_mask |= (1ULL<<(uint8_t)function); + } +} + /// Update the _aux_channels array of pointers to rc_x channels /// This is to be done before rc_init so that the channels get correctly initialized. /// It also should be called periodically because the user might change the configuration and @@ -87,47 +125,16 @@ RC_Channel_aux::Aux_servo_function_t RC_Channel_aux::channel_function(uint8_t ch /// (do not call this twice with different parameters, the second call will reset the effect of the first call) void RC_Channel_aux::update_aux_servo_function(void) { + _function_mask = 0; + // set auxiliary ranges for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] == NULL) continue; - RC_Channel_aux::Aux_servo_function_t function = (RC_Channel_aux::Aux_servo_function_t)_aux_channels[i]->function.get(); - switch (function) { - case RC_Channel_aux::k_flap: - case RC_Channel_aux::k_flap_auto: - case RC_Channel_aux::k_egg_drop: - _aux_channels[i]->set_range_out(0,100); - break; - case RC_Channel_aux::k_aileron_with_input: - case RC_Channel_aux::k_elevator_with_input: - _aux_channels[i]->set_angle(4500); - break; - case RC_Channel_aux::k_aileron: - case RC_Channel_aux::k_elevator: - case RC_Channel_aux::k_dspoiler1: - case RC_Channel_aux::k_dspoiler2: - case RC_Channel_aux::k_rudder: - case RC_Channel_aux::k_steering: - case RC_Channel_aux::k_flaperon1: - case RC_Channel_aux::k_flaperon2: - _aux_channels[i]->set_angle_out(4500); - break; - default: - break; - } - } - - // create a function mask to make updates master - _function_mask = 0; - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i]) { - RC_Channel_aux::Aux_servo_function_t function = (RC_Channel_aux::Aux_servo_function_t)_aux_channels[i]->function.get(); - if (function < k_nr_aux_servo_functions) { - _function_mask |= (1UL<<(uint8_t)function); - } - } + _aux_channels[i]->aux_servo_function_setup(); } } + /// Should be called after the the servo functions have been initialized void RC_Channel_aux::enable_aux_servos() { @@ -341,7 +348,7 @@ bool RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function) { if (function < k_nr_aux_servo_functions) { - return (_function_mask & (1UL<_ch_out == channel) { + if (_aux_channels[i]->function != k_none) { + hal.console->printf("Channel %u already assigned %u\n", + (unsigned)channel, + (unsigned)_aux_channels[i]->function); + return false; + } + _aux_channels[i]->function.set(function); + _aux_channels[i]->aux_servo_function_setup(); + return true; + } + } + hal.console->printf("AUX channel %u not available\n", + (unsigned)channel); + return false; +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index ade48e6e17..bf76e98f3f 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -66,6 +66,8 @@ public: k_epm = 28, ///< epm gripper k_landing_gear_control = 29, ///< landing gear controller k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters + k_heli_rsc = 31, ///< helicopter RSC output + k_heli_tail_rsc = 32, ///< helicopter tail RSC output k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; @@ -128,9 +130,14 @@ public: // refresh aux servo to function mapping static void update_aux_servo_function(void); + // set default channel for an auxillary function + static bool set_aux_channel_default(Aux_servo_function_t function, uint8_t channel); + private: - static uint32_t _function_mask; + static uint64_t _function_mask; static RC_Channel_aux *_aux_channels[RC_AUX_MAX_CHANNELS]; + + void aux_servo_function_setup(void); }; #endif /* RC_CHANNEL_AUX_H_ */