changes for rate and actuator chirp input for pitch and roll

ArduPilot · Aug 15, 2024 · 3d9995d · 3d9995d
1 parent 01ade70
commit 3d9995d
Show file tree

Hide file tree

Showing 7 changed files with 129 additions and 26 deletions.
diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp
@@ -24,6 +24,8 @@
 
 extern const AP_HAL::HAL& hal;
 
+AP_PitchController *AP_PitchController::singleton;
+
 const AP_Param::GroupInfo AP_PitchController::var_info[] = {
 
     // @Param: 2SRV_TCONST
@@ -164,6 +166,10 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
 AP_PitchController::AP_PitchController(const AP_FixedWing &parms)
     : aparm(parms)
 {
+    if (singleton != nullptr) {
+        AP_HAL::panic("AP_PitchController must be singleton");
+    }
+    singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
     rate_pid.set_slew_limit_scale(45);
 }
@@ -173,6 +179,10 @@ AP_PitchController::AP_PitchController(const AP_FixedWing &parms)
 */
 float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode)
 {
+    // Apply sys ID rate offset and clear
+    desired_rate += sysid.rate;
+    sysid.rate = 0;
+
     const float dt = AP::scheduler().get_loop_period_s();
 
     const AP_AHRS &_ahrs = AP::ahrs();
@@ -230,7 +240,11 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
         // when on ground suppress D and half P term to prevent oscillations
         out -= pinfo.D + 0.5*pinfo.P;
     }
-
+
+    // Apply sys ID actuator offset and clear
+    out += sysid.actuator;
+    sysid.actuator = 0;
+
     // remember the last output to trigger the I limit
     _last_out = out;
 

diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h
@@ -53,6 +53,16 @@ class AP_PitchController
 
     void convert_pid();
 
+    // Set system identification angular velocity in degrees/s
+    void set_rate_sysid(float rate) { sysid.rate = rate; }
+
+    // Set system identification actuator in range +-1.0
+    // Scale by 4500 so input range is +-1.0
+    void set_actuator_sysid(float command) { sysid.actuator = command * 4500.0; }
+
+    // Get singleton
+    static AP_PitchController *get_singleton() { return singleton;}
+
 private:
     const AP_FixedWing &aparm;
     AP_AutoTune::ATGains gains;
@@ -69,4 +79,11 @@ class AP_PitchController
 
     float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode);
     float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
+
+    struct {
+        float rate;
+        float actuator;
+    } sysid;
+
+    static AP_PitchController *singleton;
 };
diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp
@@ -25,6 +25,8 @@
 
 extern const AP_HAL::HAL& hal;
 
+AP_RollController *AP_RollController::singleton;
+
 const AP_Param::GroupInfo AP_RollController::var_info[] = {
     // @Param: 2SRV_TCONST
     // @DisplayName: Roll Time Constant
@@ -148,6 +150,10 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
 AP_RollController::AP_RollController(const AP_FixedWing &parms)
     : aparm(parms)
 {
+    if (singleton != nullptr) {
+        AP_HAL::panic("AP_RollController must be singleton");
+    }
+    singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
     rate_pid.set_slew_limit_scale(45);
 }
@@ -160,6 +166,9 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
 {
     const AP_AHRS &_ahrs = AP::ahrs();
 
+    desired_rate += sysid.rate;
+    sysid.rate = 0;
+
     const float dt = AP::scheduler().get_loop_period_s();
     const float eas2tas = _ahrs.get_EAS2TAS();
     bool limit_I = fabsf(_last_out) >= 45;
@@ -220,6 +229,9 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
     }
 
     // remember the last output to trigger the I limit
+    out += sysid.actuator;
+    sysid.actuator = 0;
+
     _last_out = out;
 
     if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {

diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h
@@ -55,6 +55,16 @@ class AP_RollController
 
     void convert_pid();
 
+    // Set system identification angular velocity in degrees/s
+    void set_rate_sysid(float rate) { sysid.rate = rate; }
+
+    // Set system identification actuator in range +-1.0
+    // Scale by 4500 so input range is +-1.0
+    void set_actuator_sysid(float command) { sysid.actuator = command * 4500.0; }
+
+    // Get singleton
+    static AP_RollController *get_singleton() { return singleton; }
+
 private:
     const AP_FixedWing &aparm;
     AP_AutoTune::ATGains gains;
@@ -68,4 +78,11 @@ class AP_RollController
     AP_PIDInfo _pid_info;
 
     float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode);
+
+    struct {
+        float rate;
+        float actuator;
+    } sysid;
+
+    static AP_RollController *singleton;
 };
diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua
@@ -3587,7 +3587,6 @@ function networking:get_netmask_active() end
 function networking:get_ip_active() end
 
 -- visual odometry object
---@class visual_odom
 visual_odom = {}
 
 -- visual odometry health
@@ -3597,3 +3596,24 @@ function visual_odom:healthy() end
 -- visual odometry quality as a percentage from 1 to 100 or 0 if unknown
 ---@return integer
 function visual_odom:quality() end
+
+-- Plane Pitch Controller
+PitchController = {}
+
+-- Inject rate demand offset into pitch controller
+---@param rate number -- rate in deg / s
+function PitchController:set_rate_sysid(rate) end
+
+-- Inject actuator offset into pitch controller
+---@param actuator number -- range of -1.0 to +1.0
+function PitchController:set_actuator_sysid(actuator) end
+
+-- Plane Roll Controller
+RollController = {}
+-- Inject rate demand offset into roll controller
+---@param rate number -- rate in deg / s
+function RollController:set_rate_sysid(rate) end
+
+-- Inject actuator offset into roll controller
+---@param actuator number -- range of -1.0 to +1.0
+function RollController:set_actuator_sysid(actuator) end
diff --git a/libraries/AP_Scripting/examples/plane-sweep.lua b/libraries/AP_Scripting/examples/plane-sweep.lua
@@ -12,10 +12,10 @@ local SWEEP_ACTION_CHANNEL = 6 -- RCIN channel to start a SWEEP when high (>1700
 local SWEEP_CHOICE_CHANNEL = 7 -- RCIN channel to choose elevator (low) or rudder (high)
 local SWEEP_FUCNTION = 1 -- which control surface (SERVOx_FUNCTION) number will have a SWEEP happen
 -- A (Servo 1, Function 4), E (Servo 2, Function 19), and R (Servo 4, Function 21)
-local SWEEP_MAGNITUDE = 50 -- defined out of 45 deg used for set_output_scaled
+local SWEEP_MAGNITUDE = 0.01 -- between 0 and 1. DO NOT SET HIGH FOR ACTUATOR FUNCTION
 local SWEEP_TIME = 120000 -- period of SWEEP signal in ms
 local start_freq = 0.05 -- start freqeuncy in hz
-local end_freqeuncy = 5.00 -- end freqeuncy in Hz
+local end_freqeuncy = 10.00 -- end freqeuncy in Hz
 local k = (end_freqeuncy-start_freq)/SWEEP_TIME -- rate of change of frequency in sweep
 local t_i = 0
 local amplitude_sent = 0
@@ -28,6 +28,8 @@ local K_ELEVATOR = 19
 local K_THROTTLE = 70
 local K_RUDDER = 21
 local SWEEP_srv_trim = -100
+local static print_cnt = 200
+local SWEEP_rc_input = 1500
 
 --- logging parameters
 local roll_rate = 1
@@ -43,7 +45,8 @@ local start_freq_rads = 2 * math.pi * start_freq
 local end_freq_rads = 2 * math.pi * end_freqeuncy
 local B = math.log(end_freq_rads / start_freq_rads)
 local k = (end_freqeuncy - start_freq)*1000 / SWEEP_TIME
-local callback_time = 40
+local callback_time = 10
+
 
 
 local interesting_data = {}
@@ -67,7 +70,7 @@ local function write_to_dataflash()
     -- not all format types are supported by scripting only: i, L, e, f, n, M, B, I, E, and N
     -- lua automatically adds a timestamp in micro seconds
     -- logger:write('SCR1','Gx(deg),Gy(deg),Gz(deg)','fff',interesting_data[roll_rate],interesting_data[pitch_rate],interesting_data[yaw_rate]) 
-    logger:write('SCR1', 'Gx,Gy,Gz,accx,accy,accz,RCOut,input,time', 'fffffffff', interesting_data[roll_rate], interesting_data[pitch_rate], interesting_data[yaw_rate],interesting_data[acc_x], interesting_data[acc_y], interesting_data[acc_z],interesting_data[RcOut],interesting_data[input],interesting_data[t_log])
+    logger:write('SIDD', 'Gx,Gy,Gz,accx,accy,accz,RCOut,input,time', 'fffffffff', interesting_data[roll_rate], interesting_data[pitch_rate], interesting_data[yaw_rate],interesting_data[acc_x], interesting_data[acc_y], interesting_data[acc_z],interesting_data[RcOut],interesting_data[input],interesting_data[t_log])
 
 
 
@@ -81,9 +84,16 @@ function SWEEP(pre_SWEEP_elevator, pre_SWEEP_aileron, pre_SWEEP_rudder, pre_SWEE
         pre_SWEEP_rudder = SRV_Channels:get_output_pwm(K_RUDDER)
         pre_SWEEP_throttle = SRV_Channels:get_output_pwm(K_THROTTLE)
     end
+    if print_cnt > 0 then
+        print_cnt = print_cnt - 1
+    else
+        print_cnt = 200
+    end
     if arming:is_armed() == true and rc:get_pwm(SWEEP_ACTION_CHANNEL) > 1700 then
-        gcs:send_text(6, "in SWEEP function")
-            pre_SWEEP_elevator = 1465
+        if print_cnt < 1 then
+            gcs:send_text(6, "in SWEEP function")
+        end
+            pre_SWEEP_elevator = SRV_Channels:get_output_pwm(K_ELEVATOR)
             pre_SWEEP_aileron = SRV_Channels:get_output_pwm(K_AILERON)
             pre_SWEEP_rudder = SRV_Channels:get_output_pwm(K_RUDDER)
             pre_SWEEP_throttle = SRV_Channels:get_output_pwm(K_THROTTLE)
@@ -93,6 +103,7 @@ function SWEEP(pre_SWEEP_elevator, pre_SWEEP_aileron, pre_SWEEP_rudder, pre_SWEE
             if SWEEP_srv_trim == -100 then
                 SWEEP_srv_trim = pre_SWEEP_elevator 
             end
+
         elseif SWEEP_choice_pwm > 1000 and SWEEP_choice_pwm < 1200 then
             SWEEP_FUCNTION = K_AILERON
             if SWEEP_srv_trim == -100 then
@@ -103,39 +114,37 @@ function SWEEP(pre_SWEEP_elevator, pre_SWEEP_aileron, pre_SWEEP_rudder, pre_SWEE
             if SWEEP_srv_trim == -100 then
                 SWEEP_srv_trim = pre_SWEEP_rudder 
             end
-        elseif SWEEP_choice_pwm > 1400 and SWEEP_choice_pwm < 1600 then
-            SWEEP_FUCNTION = K_THROTTLE
-            if SWEEP_srv_trim == -100 then
-                SWEEP_srv_trim = pre_SWEEP_throttle
-            end
         else
             SWEEP_FUCNTION = K_ELEVATOR
             if SWEEP_srv_trim == -100 then
                 SWEEP_srv_trim = pre_SWEEP_elevator 
             end
+            SWEEP_rc_input = rc:get_pwm(4)
         end
         local SWEEP_srv_chan = SRV_Channels:find_channel(SWEEP_FUCNTION)
-        local SWEEP_srv_min = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_MIN")
-        local SWEEP_srv_max = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_MAX")
-        local SWEEP_srv_trim = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_TRIM")
+        -- local SWEEP_srv_min = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_MIN")
+        -- local SWEEP_srv_max = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_MAX")
+        -- local SWEEP_srv_trim = param:get("SERVO" .. SWEEP_srv_chan + 1 .. "_TRIM")
         if start_time == -1 then
             start_time = now
             gcs:send_text(6, "STARTING SWEEP " .. SWEEP_srv_chan)
-            retry_set_mode(MODE_FBWA)
+            -- let the mode the user starts with be the mode used for sweep
+            -- retry_set_mode(MODE_FBWA)
         end
         if now < (start_time + (SWEEP_TIME)) then
-            gcs:send_text(6, "in sweep loop" .. SWEEP_srv_chan)
+            if print_cnt < 1 then
+                gcs:send_text(6, "in sweep loop" .. SWEEP_srv_chan)
+            end
             t_i = tonumber(tostring(now - start_time))/1000
             amplitude_sent = SWEEP_MAGNITUDE*math.sin(tonumber(2*math.pi*(start_freq * t_i + (k / 2) * t_i*t_i)))
-            -- amplitude_sent = SWEEP_MAGNITUDE * math.sin(start_freq_rads * (math.exp(k * t_i) - 1) / k)
-            amplitude_sent = math.floor(amplitude_sent)
-            SRV_Channels:set_output_pwm_chan_timeout(SWEEP_srv_chan,SWEEP_srv_trim + amplitude_sent,40)
-            local op = SRV_Channels:get_output_pwm(SWEEP_FUCNTION)
-            gcs:send_text(6, "pwm" .. tostring(op))
+            if SWEEP_FUCNTION == K_ELEVATOR then
+                PitchController:set_actuator_sysid(amplitude_sent)
+            elseif SWEEP_FUCNTION == K_AILERON then
+                RollController:set_actuator_sysid(amplitude_sent)
+            end
+            local op = amplitude_sent
             local rate = ahrs:get_gyro()
-            gcs:send_text(6, "rate[1]" .. tostring(rate:x()))
             local acc = ahrs:get_accel()
-            gcs:send_text(6, "acc[1]" .. tostring(acc:x()))
             if rate and acc then
                 interesting_data[roll_rate] = rate:x()
                 interesting_data[pitch_rate] = rate:y()
@@ -156,7 +165,9 @@ function SWEEP(pre_SWEEP_elevator, pre_SWEEP_aileron, pre_SWEEP_rudder, pre_SWEE
         elseif now > start_time + (SWEEP_TIME) then
             -- stick fixed response
             -- hold at pre SWEEP trim position for any damping effects
-            gcs:send_text(6, "SWEEP finished")
+            if print_cnt < 1 then
+                gcs:send_text(6, "SWEEP finished")
+            end
 
             if vehicle:get_mode() == MODE_MANUAL then
                 retry_set_mode(MODE_FBWA)

diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc
@@ -964,3 +964,15 @@ singleton AP_VisualOdom depends HAL_VISUALODOM_ENABLED
 singleton AP_VisualOdom rename visual_odom
 singleton AP_VisualOdom method healthy boolean
 singleton AP_VisualOdom method quality int8_t
+
+include APM_Control/AP_PitchController.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+singleton AP_PitchController rename PitchController
+singleton AP_PitchController depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+singleton AP_PitchController method set_rate_sysid void float'skip_check
+singleton AP_PitchController method set_actuator_sysid void float'skip_check
+
+include APM_Control/AP_RollController.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+singleton AP_RollController rename RollController
+singleton AP_RollController depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+singleton AP_RollController method set_rate_sysid void float'skip_check
+singleton AP_RollController method set_actuator_sysid void float'skip_check