From e39bc88a2aa7a564f6c7b82785cd051e5966d927 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 19 Nov 2024 13:34:57 -0300 Subject: [PATCH] Sub: refactor aux servos --- ArduSub/ArduSub.cpp | 2 + ArduSub/Parameters.cpp | 70 ++++++ ArduSub/Parameters.h | 3 + ArduSub/Sub.h | 11 + ArduSub/defines.h | 4 + ArduSub/joystick.cpp | 260 ++++++++++------------ ArduSub/servos.cpp | 61 +++++ ArduSub/system.cpp | 1 + libraries/AP_JSButton/AP_JSButton.h | 20 ++ libraries/SRV_Channel/SRV_Channel.cpp | 1 + libraries/SRV_Channel/SRV_Channel.h | 5 + libraries/SRV_Channel/SRV_Channel_aux.cpp | 4 + 12 files changed, 297 insertions(+), 145 deletions(-) create mode 100644 ArduSub/servos.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 002fbfbaad..cc5d784d62 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -159,6 +159,8 @@ void Sub::fifty_hz_loop() failsafe_sensors_check(); rc().read_input(); + + update_servos(); } // update_batt_compass - read battery and compass diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 0314f28ced..70b4b53a12 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -735,6 +735,76 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0), + // @Param: ACTUATOR1_INC + // @DisplayName: Increment step for actuator 1 + // @Description: Initial increment step for changing the actuator's PWM + // @Units: us + // @User: Standard + AP_GROUPINFO("ACTUATOR1_INC", 22, ParametersG2, actuator_increment_step[0], ACTUATOR_DEFAULT_INCREMENT), + + // @Param: ACTUATOR2_INC + // @DisplayName: Increment step for actuator 2 + // @Description: Initial increment step for changing the actuator's PWM + // @Units: us + // @User: Standard + AP_GROUPINFO("ACTUATOR2_INC", 23, ParametersG2, actuator_increment_step[1], ACTUATOR_DEFAULT_INCREMENT), + + // @Param: ACTUATOR3_INC + // @DisplayName: Increment step for actuator 3 + // @Description: Initial increment step for changing the actuator's PWM + // @Units: us + // @User: Standard + AP_GROUPINFO("ACTUATOR3_INC", 24, ParametersG2, actuator_increment_step[2], ACTUATOR_DEFAULT_INCREMENT), + + // @Param: ACTUATOR4_INC + // @DisplayName: Increment step for actuator 4 + // @Description: Initial increment step for changing the actuator's PWM + // @Units: us + // @User: Standard + AP_GROUPINFO("ACTUATOR4_INC", 25, ParametersG2, actuator_increment_step[3], ACTUATOR_DEFAULT_INCREMENT), + + // @Param: ACTUATOR5_INC + // @DisplayName: Increment step for actuator 5 + // @Description: Initial increment step for changing the actuator's PWM + // @Units: us + // @User: Standard + AP_GROUPINFO("ACTUATOR5_INC", 26, ParametersG2, actuator_increment_step[4], ACTUATOR_DEFAULT_INCREMENT), + + // @Param: ACTUATOR1_ACC + // @DisplayName: Rate at which actuator 1 change speed increases or decreases + // @Description: Rate at which actuator 5 change speed increases or decreases + // @Units: %/s + // @User: Standard + AP_GROUPINFO("ACTUATOR1_ACC", 27, ParametersG2, actuator_acceleration[0], ACTUATOR_DEFAULT_INCREMENT_ACCEL), + + // @Param: ACTUATOR2_ACC + // @DisplayName: Rate at which actuator 2 change speed increases or decreases + // @Description: Rate at which actuator 5 change speed increases or decreases + // @Units: %/s + // @User: Standard + AP_GROUPINFO("ACTUATOR2_ACC", 28, ParametersG2, actuator_acceleration[1], ACTUATOR_DEFAULT_INCREMENT_ACCEL), + + // @Param: ACTUATOR3_ACC + // @DisplayName: Rate at which actuator 3 change speed increases or decreases + // @Description: Rate at which actuator 5 change speed increases or decreases + // @Units: %/s + // @User: Standard + AP_GROUPINFO("ACTUATOR3_ACC", 29, ParametersG2, actuator_acceleration[2], ACTUATOR_DEFAULT_INCREMENT_ACCEL), + + // @Param: ACTUATOR4_ACC + // @DisplayName: Rate at which actuator 4 change speed increases or decreases + // @Description: Rate at which actuator 5 change speed increases or decreases + // @Units: %/s + // @User: Standard + AP_GROUPINFO("ACTUATOR4_ACC", 30, ParametersG2, actuator_acceleration[3], ACTUATOR_DEFAULT_INCREMENT_ACCEL), + + // @Param: ACTUATOR5_ACC + // @DisplayName: Rate at which actuator 5 change speed increases or decreases + // @Description: Rate at which actuator 5 change speed increases or decreases + // @Units: %/s + // @User: Standard + AP_GROUPINFO("ACTUATOR5_ACC", 31, ParametersG2, actuator_acceleration[4], ACTUATOR_DEFAULT_INCREMENT_ACCEL), + AP_GROUPEND }; diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index fe176e400e..1f8685e354 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -371,6 +371,9 @@ public: AP_Float backup_origin_lat; AP_Float backup_origin_lon; AP_Float backup_origin_alt; + AP_Float actuator_increment_step[ACTUATOR_CHANNELS]; + AP_Float actuator_acceleration[ACTUATOR_CHANNELS]; + }; extern const AP_Param::Info var_info[]; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 9c6e621bc4..0bb5d74727 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -256,6 +256,8 @@ private: uint8_t depth_sensor_idx; AP_Motors6DOF motors; + float aux_actuator_value[ACTUATOR_CHANNELS]; + float aux_actuator_change_speed[ACTUATOR_CHANNELS]; // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -467,6 +469,15 @@ private: void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode); void notify_flight_mode(); void read_inertia(); + void initialize_servos(); + void update_servos(); + void increase_servo(uint8_t servo_num, bool should_accelerate); + void decrease_servo(uint8_t servo_num, bool should_accelerate); + void min_servo(uint8_t servo_num); + void max_servo(uint8_t servo_num); + void min_toggle_servo(uint8_t servo_num); + void max_toggle_servo(uint8_t servo_num); + void center_servo(uint8_t servo_num); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); void set_bottomed(bool at_bottom); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index af49e7ccd1..16dbe8cd4a 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -148,3 +148,7 @@ enum LoggingParameters { #define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10) #define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11) +// if you change this, aso update SRV_CHANNEL.h to add new enum entries +#define ACTUATOR_CHANNELS 5 +#define ACTUATOR_DEFAULT_INCREMENT_ACCEL 0.001 +#define ACTUATOR_DEFAULT_INCREMENT 0.001 diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 5e12c0eeae..47366c062d 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -443,177 +443,147 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) //////////////////////////////////////////////// // Servo functions - // TODO: initialize #if AP_SERVORELAYEVENTS_ENABLED case JSButton::button_function_t::k_servo_1_inc: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed - } + increase_servo(0, held); break; - case JSButton::button_function_t::k_servo_1_dec: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed - } + case JSButton::button_function_t::k_servo_2_inc: + increase_servo(1, held); break; - case JSButton::button_function_t::k_servo_1_min: - case JSButton::button_function_t::k_servo_1_min_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed - } + case JSButton::button_function_t::k_servo_3_inc: + increase_servo(2, held); break; - case JSButton::button_function_t::k_servo_1_min_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_min()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed - } - } + case JSButton::button_function_t::k_servo_4_inc: + increase_servo(3, held); break; - case JSButton::button_function_t::k_servo_1_max: - case JSButton::button_function_t::k_servo_1_max_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed - } - break; - case JSButton::button_function_t::k_servo_1_max_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_max()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed - } - } - break; - case JSButton::button_function_t::k_servo_1_center: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed - } + case JSButton::button_function_t::k_servo_5_inc: + increase_servo(4, held); break; - case JSButton::button_function_t::k_servo_2_inc: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed - } + case JSButton::button_function_t::k_servo_1_dec: + decrease_servo(0, held); break; case JSButton::button_function_t::k_servo_2_dec: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed - } + decrease_servo(1, held); + break; + case JSButton::button_function_t::k_servo_3_dec: + decrease_servo(2, held); + break; + case JSButton::button_function_t::k_servo_4_dec: + decrease_servo(3, held); + break; + case JSButton::button_function_t::k_servo_5_dec: + decrease_servo(4, held); + break; + + case JSButton::button_function_t::k_servo_1_min: + case JSButton::button_function_t::k_servo_1_min_momentary: + min_servo(0); break; case JSButton::button_function_t::k_servo_2_min: case JSButton::button_function_t::k_servo_2_min_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed - } - break; - case JSButton::button_function_t::k_servo_2_min_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_min()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed - } - } - break; - case JSButton::button_function_t::k_servo_2_max: - case JSButton::button_function_t::k_servo_2_max_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed - } - break; - case JSButton::button_function_t::k_servo_2_max_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_max()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed - } - } - break; - case JSButton::button_function_t::k_servo_2_center: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed - } - break; - - case JSButton::button_function_t::k_servo_3_inc: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed - } - break; - case JSButton::button_function_t::k_servo_3_dec: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed - pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed - } + min_servo(1); break; case JSButton::button_function_t::k_servo_3_min: case JSButton::button_function_t::k_servo_3_min_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed - } + min_servo(2); + break; + case JSButton::button_function_t::k_servo_4_min: + case JSButton::button_function_t::k_servo_4_min_momentary: + min_servo(3); + break; + case JSButton::button_function_t::k_servo_5_min: + case JSButton::button_function_t::k_servo_5_min_momentary: + min_servo(4); + break; + + case JSButton::button_function_t::k_servo_1_min_toggle: + if (!held) { + min_toggle_servo(0); + } + break; + case JSButton::button_function_t::k_servo_2_min_toggle: + if (!held) { + min_toggle_servo(1); + } break; case JSButton::button_function_t::k_servo_3_min_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_min()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed - } + if (!held) { + min_toggle_servo(2); } break; + case JSButton::button_function_t::k_servo_4_min_toggle: + if (!held) { + min_toggle_servo(3); + } + break; + case JSButton::button_function_t::k_servo_5_min_toggle: + if (!held) { + min_toggle_servo(4); + } + break; + + case JSButton::button_function_t::k_servo_1_max: + case JSButton::button_function_t::k_servo_1_max_momentary: + max_servo(0); + break; + case JSButton::button_function_t::k_servo_2_max: + case JSButton::button_function_t::k_servo_2_max_momentary: + max_servo(1); + break; case JSButton::button_function_t::k_servo_3_max: case JSButton::button_function_t::k_servo_3_max_momentary: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed - } + max_servo(2); break; - case JSButton::button_function_t::k_servo_3_max_toggle: - if(!held) { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - if(chan->get_output_pwm() != chan->get_output_max()) { - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed - } else { - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed - } + case JSButton::button_function_t::k_servo_4_max: + case JSButton::button_function_t::k_servo_4_max_momentary: + max_servo(3); + break; + case JSButton::button_function_t::k_servo_5_max: + case JSButton::button_function_t::k_servo_5_max_momentary: + max_servo(4); + break; + + case JSButton::button_function_t::k_servo_1_max_toggle: + if (!held) { + max_toggle_servo(0); } break; + case JSButton::button_function_t::k_servo_2_max_toggle: + if (!held) { + max_toggle_servo(1); + } + break; + case JSButton::button_function_t::k_servo_3_max_toggle: + if (!held) { + max_toggle_servo(2); + } + break; + case JSButton::button_function_t::k_servo_4_max_toggle: + if (!held) { + max_toggle_servo(3); + } + break; + case JSButton::button_function_t::k_servo_5_max_toggle: + if (!held) { + max_toggle_servo(4); + } + break; + + case JSButton::button_function_t::k_servo_1_center: + center_servo(0); + break; + case JSButton::button_function_t::k_servo_2_center: + center_servo(1); + break; case JSButton::button_function_t::k_servo_3_center: - { - SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed - ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed - } + center_servo(2); + break; + case JSButton::button_function_t::k_servo_4_center: + center_servo(3); + break; + case JSButton::button_function_t::k_servo_5_center: + center_servo(4); break; #endif // AP_SERVORELAYEVENTS_ENABLED diff --git a/ArduSub/servos.cpp b/ArduSub/servos.cpp new file mode 100644 index 0000000000..dc70c12757 --- /dev/null +++ b/ArduSub/servos.cpp @@ -0,0 +1,61 @@ +#include "Sub.h" + +void Sub::initialize_servos() { + const SRV_Channel::Aux_servo_function_t first_aux = SRV_Channel::Aux_servo_function_t::k_actuator1; + for (int i = 0; i < ACTUATOR_CHANNELS; i++) { + SRV_Channels::set_output_scaled((SRV_Channel::Aux_servo_function_t) ((int16_t) first_aux + i), 0); + } +} + +void Sub::update_servos() { + const SRV_Channel::Aux_servo_function_t first_aux = SRV_Channel::Aux_servo_function_t::k_actuator1; + for (int i = 0; i < ACTUATOR_CHANNELS; i++) { + SRV_Channels::set_output_scaled((SRV_Channel::Aux_servo_function_t) ((int16_t) first_aux + i), aux_actuator_value[i]); + } +} + +void Sub::increase_servo(uint8_t servo_num, bool should_accelerate) { + if (!should_accelerate) { + aux_actuator_change_speed[servo_num] = g2.actuator_increment_step[servo_num]; + } else { + aux_actuator_change_speed[servo_num] = constrain_float(aux_actuator_change_speed[servo_num] + g2.actuator_increment_step[servo_num], -1, 1); + } + aux_actuator_value[servo_num] = constrain_float(aux_actuator_value[servo_num] + aux_actuator_change_speed[servo_num], -1, 1); +} + +void Sub::decrease_servo(uint8_t servo_num, bool should_accelerate) { + if (!should_accelerate) { + aux_actuator_change_speed[servo_num] = g2.actuator_increment_step[servo_num]; + } else { + aux_actuator_change_speed[servo_num] = constrain_float(aux_actuator_change_speed[servo_num] + g2.actuator_increment_step[servo_num], -1, 1); + } + aux_actuator_value[servo_num] = constrain_float(aux_actuator_value[servo_num] - aux_actuator_change_speed[servo_num], -1, 1); +} + +void Sub::min_servo(uint8_t servo_num) { + aux_actuator_value[servo_num] = -1; +} + +void Sub::max_servo(uint8_t servo_num) { + aux_actuator_value[servo_num] = 1; +} + +void Sub::min_toggle_servo(uint8_t servo_num) { + if (aux_actuator_value[servo_num] > -1) { + aux_actuator_value[servo_num] = -1; + } else { + aux_actuator_value[servo_num] = 0; + } +} + +void Sub::max_toggle_servo(uint8_t servo_num) { + if (aux_actuator_value[servo_num] < 1) { + aux_actuator_value[servo_num] = 1; + } else { + aux_actuator_value[servo_num] = 0; + } +} + +void Sub::center_servo(uint8_t servo_num) { + aux_actuator_value[servo_num] = 0; +} diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 72803e6d47..415f97e999 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -155,6 +155,7 @@ void Sub::init_ardupilot() mainloop_failsafe_enable(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); + initialize_servos(); // flag that initialisation has completed ap.initialised = true; diff --git a/libraries/AP_JSButton/AP_JSButton.h b/libraries/AP_JSButton/AP_JSButton.h index 19e1879f05..4c97877ef9 100644 --- a/libraries/AP_JSButton/AP_JSButton.h +++ b/libraries/AP_JSButton/AP_JSButton.h @@ -118,6 +118,26 @@ public: k_script_3 = 110, k_script_4 = 111, + k_servo_4_min = 112, + k_servo_4_max = 113, + k_servo_4_center = 114, + k_servo_4_inc = 115, + k_servo_4_dec = 116, + k_servo_4_min_momentary = 117, + k_servo_4_max_momentary = 118, + k_servo_4_min_toggle = 119, + k_servo_4_max_toggle = 120, + + k_servo_5_min = 121, + k_servo_5_max = 122, + k_servo_5_center = 123, + k_servo_5_inc = 124, + k_servo_5_dec = 125, + k_servo_5_min_momentary = 126, + k_servo_5_max_momentary = 127, + k_servo_5_min_toggle = 128, + k_servo_5_max_toggle = 129, + // 112+ reserved for future functions k_nr_btn_functions ///< This must be the last enum value (only add new values _before_ this one) } button_function_t; diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 1cfccbd8bd..433a12e10a 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -156,6 +156,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Values{Plane, Copter, Rover}: 138:Alarm,139:Alarm Inverted // @Values: 140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled // @Values{Plane, Copter, Rover}: 140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled + // @Values{Sub}: 157: Actuator1,158: Actuator2,159: Actuator3,160: Actuator4,161: Actuator5,162: Actuator6,163: Actuator7,164: Actuator8 // @User: Standard // @RebootRequired: True AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index acb6021b89..43ba00e75d 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -189,6 +189,11 @@ public: k_rcin15_mapped = 154, k_rcin16_mapped = 155, k_lift_release = 156, + k_actuator1 = 157, // Aux channels used for controlling user peripherals + k_actuator2 = 158, + k_actuator3 = 159, + k_actuator4 = 160, + k_actuator5 = 161, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 3fb4e92274..79ffd09bbe 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -200,6 +200,10 @@ void SRV_Channel::aux_servo_function_setup(void) // fixed wing throttle set_range(100); break; + case k_actuator1 ... k_actuator5: + // We take floats from -1 to 1. see MAV_CMD_DO_SET_ACTUATOR + set_angle(1); + break; default: break; }