Sub: refactor aux servos

Williangalvani · Dec 4, 2024 · d6a5bd4 · d6a5bd4
1 parent 79b596d
commit d6a5bd4
Show file tree

Hide file tree

Showing 12 changed files with 295 additions and 139 deletions.
diff --git 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
@@ -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:  
+    // @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:  
+    // @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:  
+    // @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:  
+    // @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:  
+    // @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:
+    // @Units: us/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:
+    // @Units: us/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:
+    // @Units: us/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:
+    // @Units: us/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:
+    // @Units: us/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
@@ -371,6 +371,9 @@ class ParametersG2 {
     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
@@ -256,6 +256,8 @@ class Sub : public AP_Vehicle {
     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 @@ class Sub : public AP_Vehicle {
     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
@@ -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