Blimp: Add logging and MAVLink

Blimp/Blimp.cpp

@@ -40,7 +40,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_ServoRelayEvents,  &blimp.ServoRelayEvents,      update_events, 50,     75),
     SCHED_TASK_CLASS(AP_Baro,              &blimp.barometer,           accumulate,      50,  90),
 #if LOGGING_ENABLED == ENABLED
-    SCHED_TASK(fourhundred_hz_logging,400,    50),
+    SCHED_TASK(full_rate_logging,     50,    50),
 #endif
     SCHED_TASK_CLASS(AP_Notify,            &blimp.notify,              update,          50,  90),
     SCHED_TASK(one_hz_loop,            1,    100),
@@ -140,8 +140,7 @@ void Blimp::update_batt_compass(void)
 }
 
 // Full rate logging of attitude, rate and pid loops
-// should be run at 400hz
+void Blimp::full_rate_logging()
-void Blimp::fourhundred_hz_logging()
 {
     if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
         Log_Write_Attitude();

Blimp/Blimp.h

@@ -307,7 +307,7 @@ private:
     void rc_loop();
     void throttle_loop();
     void update_batt_compass(void);
-    void fourhundred_hz_logging();
+    void full_rate_logging();
     void ten_hz_logging_loop();
     void twentyfive_hz_logging();
     void three_hz_loop();
@@ -361,6 +361,7 @@ private:
     // Log.cpp
     void Log_Write_Performance();
     void Log_Write_Attitude();
+    void Log_Write_PIDs();
     void Log_Write_EKF_POS();
     void Log_Write_MotBatt();
     void Log_Write_Data(LogDataID id, int32_t value);
@@ -375,6 +376,8 @@ private:
     void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
     void Log_Write_Vehicle_Startup_Messages();
     void log_init(void);
+    void Write_FINI(float right, float front, float down, float yaw);
+    void Write_FINO(float *amp, float *off);
 
     // mode.cpp
     bool set_mode(Mode::Number mode, ModeReason reason);

Blimp/Fins.cpp

@@ -78,6 +78,7 @@ void Fins::output()
         yaw_out   = 0;
     }
 
+    blimp.Write_FINI(right_out, front_out, down_out, yaw_out);
 
     _time = AP_HAL::micros() * 1.0e-6;
 
@@ -121,9 +122,7 @@ void Fins::output()
         SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX);
     }
 
-    //For debugging purposes. Displays in the debug terminal so it doesn't flood the GCS.
+    blimp.Write_FINO(_amp, _off);
-    ::printf("FINS (amp %.1f %.1f %.1f %.1f   off %.1f %.1f %.1f %.1f   omm %.1f %.1f %.1f %.1f)\n",
-             _amp[0], _amp[1], _amp[2], _amp[3], _off[0], _off[1], _off[2], _off[3], _omm[0], _omm[1], _omm[2], _omm[3]);
 }
 
 void Fins::output_min()

Blimp/GCS_Mavlink.cpp

@@ -111,11 +111,20 @@ int16_t GCS_MAVLINK_Blimp::vfr_hud_throttle() const
  */
 void GCS_MAVLINK_Blimp::send_pid_tuning()
 {
-    static const PID_TUNING_AXIS axes[] = {
+    if(blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) {
-        PID_TUNING_ROLL,
+        //No PIDs are used in Manual or Land mode.
-        PID_TUNING_PITCH,
+        return;
-        PID_TUNING_YAW,
+    }
-        PID_TUNING_ACCZ
+    
+    static const int8_t axes[] = {
+        PID_SEND::VELX,
+        PID_SEND::VELY,
+        PID_SEND::VELZ,
+        PID_SEND::VELYAW,
+        PID_SEND::POSX,
+        PID_SEND::POSY,
+        PID_SEND::POSZ,
+        PID_SEND::POSYAW
     };
     for (uint8_t i=0; i<ARRAY_SIZE(axes); i++) {
         if (!(blimp.g.gcs_pid_mask & (1<<(axes[i]-1)))) {
@@ -125,19 +134,31 @@ void GCS_MAVLINK_Blimp::send_pid_tuning()
             return;
         }
         const AP_Logger::PID_Info *pid_info = nullptr;
-        switch (axes[i]) { //TODO This should probably become an acceleration controller?
+        switch (axes[i]) {
-        // case PID_TUNING_ROLL:
+        case PID_SEND::VELX:
-        //     pid_info = &blimp.attitude_control->get_rate_roll_pid().get_pid_info();
+            pid_info = &blimp.pid_vel_xy.get_pid_info_x();
-        //     break;
+            break;
-        // case PID_TUNING_PITCH:
+        case PID_SEND::VELY:
-        //     pid_info = &blimp.attitude_control->get_rate_pitch_pid().get_pid_info();
+            pid_info = &blimp.pid_vel_xy.get_pid_info_y();
-        //     break;
+            break;
-        // case PID_TUNING_YAW:
+        case PID_SEND::VELZ:
-        //     pid_info = &blimp.attitude_control->get_rate_yaw_pid().get_pid_info();
+            pid_info = &blimp.pid_vel_z.get_pid_info();
-        //     break;
+            break;
-        // case PID_TUNING_ACCZ:
+        case PID_SEND::VELYAW:
-        //     pid_info = &blimp.pos_control->get_accel_z_pid().get_pid_info();
+            pid_info = &blimp.pid_vel_yaw.get_pid_info();
-        //     break;
+            break;
+        case PID_SEND::POSX:
+            pid_info = &blimp.pid_pos_xy.get_pid_info_x();
+            break;
+        case PID_SEND::POSY:
+            pid_info = &blimp.pid_pos_xy.get_pid_info_y();
+            break;
+        case PID_SEND::POSZ:
+            pid_info = &blimp.pid_pos_z.get_pid_info();
+            break;
+        case PID_SEND::POSYAW:
+            pid_info = &blimp.pid_pos_yaw.get_pid_info();
+            break;
         default:
             continue;
         }

Blimp/GCS_Mavlink.h

@@ -64,4 +64,16 @@ private:
     void send_pid_tuning() override;
 
     void send_wind() const;
+
+    //This is 1-indexed, unlike most enums for consistency with the mavlink PID_TUNING enums.
+    enum PID_SEND : uint8_t {
+        VELX =        1,  
+        VELY =        2, 
+        VELZ =        3,
+        VELYAW =      4, 
+        POSX =        5,
+        POSY =        6,
+        POSZ =        7,
+        POSYAW =      8,
+    };
 };

Blimp/Log.cpp

@@ -5,6 +5,60 @@
 // Code to Write and Read packets from AP_Logger log memory
 // Code to interact with the user to dump or erase logs
 
+struct PACKED log_FINI {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float Right;
+    float Front;
+    float Down;
+    float Yaw;
+};
+
+struct PACKED log_FINO {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float Fin1_Amp;
+    float Fin1_Off;
+    float Fin2_Amp;
+    float Fin2_Off;
+    float Fin3_Amp;
+    float Fin3_Off;
+    float Fin4_Amp;
+    float Fin4_Off;
+};
+
+//Write a fin input packet
+void Blimp::Write_FINI(float right, float front, float down, float yaw)
+{
+    const struct log_FINI pkt{
+        LOG_PACKET_HEADER_INIT(LOG_FINI_MSG),
+        time_us       : AP_HAL::micros64(),
+        Right         : right,
+        Front         : front,
+        Down          : down,
+        Yaw           : yaw
+    };
+    logger.WriteBlock(&pkt, sizeof(pkt));
+}
+
+//Write a fin output packet
+void Blimp::Write_FINO(float *amp, float *off)
+{
+    const struct log_FINO pkt{
+        LOG_PACKET_HEADER_INIT(LOG_FINO_MSG),
+        time_us       : AP_HAL::micros64(),
+        Fin1_Amp      : amp[0],
+        Fin1_Off      : off[0],
+        Fin2_Amp      : amp[1],
+        Fin2_Off      : off[1],
+        Fin3_Amp      : amp[2],
+        Fin3_Off      : off[2],
+        Fin4_Amp      : amp[3],
+        Fin4_Off      : off[3],
+    };
+    logger.WriteBlock(&pkt, sizeof(pkt));
+}
+
 struct PACKED log_Control_Tuning {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -22,10 +76,17 @@ struct PACKED log_Control_Tuning {
     int16_t  climb_rate;
 };
 
-// Write a control tuning packet
+// Write PID packets
-void Blimp::Log_Write_Control_Tuning()
+void Blimp::Log_Write_PIDs()
 {
-
+    logger.Write_PID(LOG_PIVN_MSG, pid_vel_xy.get_pid_info_x());
+    logger.Write_PID(LOG_PIVE_MSG, pid_vel_xy.get_pid_info_y());
+    logger.Write_PID(LOG_PIVD_MSG, pid_vel_z.get_pid_info());
+    logger.Write_PID(LOG_PIVY_MSG, pid_vel_yaw.get_pid_info());
+    logger.Write_PID(LOG_PIDN_MSG, pid_pos_xy.get_pid_info_x());
+    logger.Write_PID(LOG_PIDE_MSG, pid_pos_xy.get_pid_info_y());
+    logger.Write_PID(LOG_PIDD_MSG, pid_pos_z.get_pid_info());
+    logger.Write_PID(LOG_PIDY_MSG, pid_pos_yaw.get_pid_info());
 }
 
 // Write an attitude packet
@@ -288,6 +349,56 @@ time_fade_out       : time_fade_out
 const struct LogStructure Blimp::log_structure[] = {
     LOG_COMMON_STRUCTURES,
 
+    // @LoggerMessage: FINI
+    // @Description: Fin input
+    // @Field: TimeUS: Time since system startup
+    // @Field: R: Right
+    // @Field: F: Front
+    // @Field: D: Down
+    // @Field: Y: Yaw 
+
+    { LOG_FINI_MSG, sizeof(log_FINI),
+      "FINI",  "Qffff",     "TimeUS,R,F,D,Y", "s----", "F----"  }, 
+
+    // @LoggerMessage: FINO
+    // @Description: Fin output
+    // @Field: TimeUS: Time since system startup
+    // @Field: F1A: Fin 1 Amplitude
+    // @Field: F1O: Fin 1 Offset
+    // @Field: F2A: Fin 2 Amplitude
+    // @Field: F2O: Fin 2 Offset
+    // @Field: F3A: Fin 3 Amplitude
+    // @Field: F3O: Fin 3 Offset
+    // @Field: F4A: Fin 4 Amplitude
+    // @Field: F4O: Fin 4 Offset
+
+    { LOG_FINO_MSG, sizeof(log_FINO),
+      "FINO",  "Qffffffff",     "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------"  },
+
+    // @LoggerMessage: PIDD,PIVN,PIVE,PIVD,PIVY
+    // @Description: Proportional/Integral/Derivative gain values
+    // @Field: TimeUS: Time since system startup
+    // @Field: Tar: desired value
+    // @Field: Act: achieved value
+    // @Field: Err: error between target and achieved
+    // @Field: P: proportional part of PID
+    // @Field: I: integral part of PID
+    // @Field: D: derivative part of PID
+    // @Field: FF: controller feed-forward portion of response
+    // @Field: Dmod: scaler applied to D gain to reduce limit cycling
+    // @Field: SRate: slew rate
+    // @Field: Limit: 1 if I term is limited due to output saturation
+    { LOG_PIDD_MSG, sizeof(log_PID),
+      "PIDD", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS },
+    { LOG_PIVN_MSG, sizeof(log_PID),
+      "PIVN", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS },
+    { LOG_PIVE_MSG, sizeof(log_PID),
+      "PIVE", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS },
+    { LOG_PIVD_MSG, sizeof(log_PID),
+      "PIVD", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS },
+    { LOG_PIVY_MSG, sizeof(log_PID),
+      "PIVY", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS },
+
     // @LoggerMessage: PTUN
     // @Description: Parameter Tuning information
     // @URL: https://ardupilot.org/blimp/docs/tuning.html#in-flight-tuning
@@ -454,6 +565,7 @@ void Blimp::log_init(void)
 
 void Blimp::Log_Write_Performance() {}
 void Blimp::Log_Write_Attitude(void) {}
+void Blimp::Log_Write_PIDs(void) {}
 void Blimp::Log_Write_EKF_POS() {}
 void Blimp::Log_Write_MotBatt() {}
 void Blimp::Log_Write_Data(LogDataID id, int32_t value) {}

Blimp/Parameters.cpp

@@ -95,9 +95,9 @@ const AP_Param::Info Blimp::var_info[] = {
     // @DisplayName: GCS PID tuning mask
     // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
     // @User: Advanced
-    // @Values: 0:None,1:Roll,2:Pitch,4:Yaw,8:AccelZ
+    // @Values: 0:None,1:VELX,2:VELY,4:VELZ,8:VELYAW,16:POSX,32:POSY,64:POSZ,128:POSYAW,15:Vel only,51:XY only,204:ZYaw only,240:Pos only,255:All
-    // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ
+    // @Bitmask: 0:VELX,1:VELY,2:VELZ;3:VELYAW;4:POSX,5:POSY;6:POZ;7:POSYAW
-    GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
+    GSCALAR(gcs_pid_mask, "GCS_PID_MASK",     0),
 
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
@@ -520,6 +520,5 @@ void Blimp::load_parameters(void)
     hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
 
     // setup AP_Param frame type flags
-    AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
+    AP_Param::set_frame_type_flags(AP_PARAM_FRAME_BLIMP);
-
 }

Blimp/defines.h

@@ -138,6 +138,14 @@ enum LoggingParameters {
     LOG_GUIDEDTARGET_MSG,
     LOG_SYSIDD_MSG,
     LOG_SYSIDS_MSG,
+    LOG_FINI_MSG,
+    LOG_FINO_MSG,
+    LOG_PIDD_MSG,
+    LOG_PIVN_MSG,
+    LOG_PIVE_MSG,
+    LOG_PIVD_MSG,
+    LOG_PIVY_MSG,
+
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)