Plane: added in-flight transmitter tuning

ArduPlane/Log.cpp

@@ -482,7 +482,7 @@ void Plane::Log_Write_Home_And_Origin()
     }
 }
 
-static const struct LogStructure log_structure[] = {
+const struct LogStructure Plane::log_structure[] = {
     LOG_COMMON_STRUCTURES,
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
       "PM",  "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
@@ -506,6 +506,8 @@ static const struct LogStructure log_structure[] = {
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
       "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },
 #endif
+    { LOG_PARAMTUNE_MSG, sizeof(Tuning::log_ParameterTuning),
+      "PTUN", "QBfff",          "TimeUS,Param,TunVal,TunLo,TunHi" },  
     TECS_LOG_FORMAT(LOG_TECS_MSG)
 };
 

ArduPlane/Parameters.cpp

@@ -1106,6 +1106,10 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: quadplane.cpp
     GOBJECT(quadplane,           "Q_", QuadPlane),
 
+    // @Group: TUNE_
+    // @Path: tuning.cpp
+    GOBJECT(tuning,           "TUNE_", Tuning),
+    
     // @Group: Q_A_
     // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
     { AP_PARAM_GROUP, "Q_A_", Parameters::k_param_q_attitude_control,

ArduPlane/Parameters.h

@@ -272,6 +272,7 @@ public:
         k_param_long_fs_timeout,
         k_param_rc_13,
         k_param_rc_14,
+        k_param_tuning,
 
         //
         // 200: Feed-forward gains

ArduPlane/Plane.h

@@ -95,6 +95,7 @@
 #include <AP_ADSB/AP_ADSB.h>
 
 #include "quadplane.h"
+#include "tuning.h"
 
 // Configuration
 #include "config.h"
@@ -136,6 +137,7 @@ public:
     friend class Parameters;
     friend class AP_Arming_Plane;
     friend class QuadPlane;
+    friend class Tuning;
 
     Plane(void);
 
@@ -729,6 +731,11 @@ private:
     // support for quadcopter-plane
     QuadPlane quadplane{ahrs};
 
+    // support for transmitter tuning
+    Tuning tuning;
+
+    static const struct LogStructure log_structure[];
+    
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     // the crc of the last created PX4Mixer
     int32_t last_mixer_crc = -1;

ArduPlane/defines.h

@@ -117,7 +117,8 @@ enum log_messages {
     LOG_ARM_DISARM_MSG,
     LOG_STATUS_MSG,
     LOG_OPTFLOW_MSG,
-    LOG_QTUN_MSG
+    LOG_QTUN_MSG,
+    LOG_PARAMTUNE_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

ArduPlane/quadplane.h

@@ -14,6 +14,7 @@ class QuadPlane
 {
 public:
     friend class Plane;
+    friend class Tuning;
     QuadPlane(AP_AHRS_NavEKF &_ahrs);
 
     // var_info for holding Parameter information

ArduPlane/radio.cpp

@@ -215,6 +215,8 @@ void Plane::read_radio()
     } else {
         rudder_input = channel_rudder->control_in;
     }
+
+    tuning.check_input();
 }
 
 void Plane::control_failsafe(uint16_t pwm)

ArduPlane/tuning.cpp

@@ -0,0 +1,223 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+const AP_Param::GroupInfo Tuning::var_info[] = {
+
+    // @Param: CHAN
+    // @DisplayName: Transmitter tuning channel
+    // @Description: This sets the channel for transmitter tuning 
+    // @Values: 0:Disable,1:Chan1,2:Chan3,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
+    // @User: Standard
+    AP_GROUPINFO_FLAGS("CHAN", 1, Tuning, channel, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: PARM
+    // @DisplayName: Transmitter tuning parameter
+    // @Description: This sets which parameter or combination of parameters will be tuned
+    // @User: Standard
+    AP_GROUPINFO("PARM", 2, Tuning, parm, 0),
+
+    // @Param: MIN
+    // @DisplayName: Transmitter tuning minimum value
+    // @Description: This sets the minimum value
+    // @User: Standard
+    AP_GROUPINFO("MIN", 3, Tuning, minimum, 0),
+
+    // @Param: MAX
+    // @DisplayName: Transmitter tuning maximum value
+    // @Description: This sets the maximum value
+    // @User: Standard
+    AP_GROUPINFO("MAX", 4, Tuning, maximum, 0),
+    
+    AP_GROUPEND
+};
+
+/*
+  constructor
+ */
+Tuning::Tuning(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/*
+  check for changed tuning input
+ */
+void Tuning::check_input(void)
+{
+    if (channel <= 0 || parm <= 0) {
+        // disabled
+        return;
+    }
+
+    // only adjust values at 4Hz
+    uint32_t now = AP_HAL::millis();
+    if (now - last_check_ms < 250) {
+        return;
+    }
+    last_check_ms = now;
+
+    if (channel > hal.rcin->num_channels()) {
+        return;
+    }
+    
+    RC_Channel *chan = RC_Channel::rc_channel(channel-1);
+    if (chan == nullptr) {
+        return;
+    }
+    uint8_t input = chan->percent_input();
+    if (input == last_input_pct) {
+        // no change
+        return;
+    }
+    last_input_pct = input;
+
+    float tuning_value = minimum + (maximum-minimum)*(input*0.01f);
+
+    if (!plane.quadplane.available()) {
+        // quadplane tuning options not available
+        return;
+    }
+
+    switch((enum tuning_func)parm.get()) {
+
+    case TUNING_Q_RATE_ROLL_PITCH_KPI:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_PITCH_KP:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_PITCH_KI:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_PITCH_KD:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_KPI:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_KP:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_KI:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_ROLL_KD:
+        plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_PITCH_KPI:
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_PITCH_KP:
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_PITCH_KI:
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_PITCH_KD:
+        plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_YAW_KPI:
+        plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value);
+        plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_YAW_KP:
+        plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_YAW_KI:
+        plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value);
+        break;
+
+    case TUNING_Q_RATE_YAW_KD:
+        plane.quadplane.attitude_control->get_rate_yaw_pid().kD(tuning_value);
+        break;
+
+    case TUNING_Q_ANG_ROLL_KP:
+        plane.quadplane.attitude_control->get_angle_roll_p().kP(tuning_value);
+        break;
+
+    case TUNING_Q_ANG_PITCH_KP:
+        plane.quadplane.attitude_control->get_angle_pitch_p().kP(tuning_value);
+        break;
+
+    case TUNING_Q_ANG_YAW_KP:
+        plane.quadplane.attitude_control->get_angle_yaw_p().kP(tuning_value);
+        break;
+
+    case TUNING_Q_PXY_P:
+        plane.quadplane.p_pos_xy.kP(tuning_value);
+        break;
+
+    case TUNING_Q_PZ_P:
+        plane.quadplane.p_alt_hold.kP(tuning_value);
+        break;
+
+    case TUNING_Q_VXY_P:
+        plane.quadplane.pi_vel_xy.kP(tuning_value);
+        break;
+
+    case TUNING_Q_VXY_I:
+        plane.quadplane.pi_vel_xy.kI(tuning_value);
+        break;
+
+    case TUNING_Q_VZ_P:
+        plane.quadplane.p_vel_z.kP(tuning_value);
+        break;
+
+    case TUNING_Q_AZ_P:
+        plane.quadplane.pid_accel_z.kP(tuning_value);
+        break;
+
+    case TUNING_Q_AZ_I:
+        plane.quadplane.pid_accel_z.kI(tuning_value);
+        break;
+
+    case TUNING_Q_AZ_D:
+        plane.quadplane.pid_accel_z.kD(tuning_value);
+        break;
+        
+    default:
+        return;
+    }
+
+    Log_Write_Parameter_Tuning((uint8_t)parm.get(), tuning_value, minimum, maximum);
+}
+
+/*
+  log a tuning change
+ */
+void Tuning::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high)
+{
+    struct log_ParameterTuning pkt_tune = {
+        LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
+        time_us        : AP_HAL::micros64(),
+        parameter      : param,
+        tuning_value   : tuning_val,
+        tuning_low     : tune_low,
+        tuning_high    : tune_high
+    };
+
+    plane.DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
+}

ArduPlane/tuning.h

@@ -0,0 +1,75 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+  Plane transmitter tuning
+ */
+class Tuning
+{
+public:
+    friend class Plane;
+
+    Tuning(void);
+    
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    void check_input(void);
+    
+private:
+    AP_Int8 channel;
+    AP_Int8 parm;
+    AP_Float minimum;
+    AP_Float maximum;
+
+    uint8_t last_input_pct = 255;
+    uint32_t last_check_ms;
+
+    struct PACKED log_ParameterTuning {
+        LOG_PACKET_HEADER;
+        uint64_t time_us;
+        uint8_t  parameter;     // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE
+        float    tuning_value;  // normalized value used inside tuning() function
+        float    tuning_low;    // tuning low end value
+        float    tuning_high;   // tuning high end value
+    };
+
+    void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high);
+    
+    enum tuning_func {
+        TUNING_NONE =                         0,
+        TUNING_Q_RATE_ROLL_PITCH_KPI =         1,
+        TUNING_Q_RATE_ROLL_PITCH_KP =          2,
+        TUNING_Q_RATE_ROLL_PITCH_KI =          3,
+        TUNING_Q_RATE_ROLL_PITCH_KD =          4,
+
+        TUNING_Q_RATE_ROLL_KPI =               5,
+        TUNING_Q_RATE_ROLL_KP =                6,
+        TUNING_Q_RATE_ROLL_KI =                7,
+        TUNING_Q_RATE_ROLL_KD =                8,
+
+        TUNING_Q_RATE_PITCH_KPI =              9,
+        TUNING_Q_RATE_PITCH_KP =              10,
+        TUNING_Q_RATE_PITCH_KI =              11,
+        TUNING_Q_RATE_PITCH_KD =              12,
+
+        TUNING_Q_RATE_YAW_KPI =               13,
+        TUNING_Q_RATE_YAW_KP =                14,
+        TUNING_Q_RATE_YAW_KI =                15,
+        TUNING_Q_RATE_YAW_KD =                16,
+
+        TUNING_Q_ANG_ROLL_KP =                17,
+        TUNING_Q_ANG_PITCH_KP =               18,
+        TUNING_Q_ANG_YAW_KP =                 19,
+
+        TUNING_Q_PXY_P =                      20,
+        TUNING_Q_PZ_P  =                      21,
+
+        TUNING_Q_VXY_P =                      22,
+        TUNING_Q_VXY_I =                      23,
+        TUNING_Q_VZ_P  =                      24,
+
+        TUNING_Q_AZ_P =                       25,
+        TUNING_Q_AZ_I =                       26,
+        TUNING_Q_AZ_D  =                      27,
+    };
+};