diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 977c9d1058..697e16f7e2 100644 --- a/Blimp/Blimp.cpp +++ b/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::fourhundred_hz_logging() +void Blimp::full_rate_logging() { if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { Log_Write_Attitude(); diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 878fbb0490..e6dcce983b 100644 --- a/Blimp/Blimp.h +++ b/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); diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index 2fba13b01c..631b678ee7 100644 --- a/Blimp/Fins.cpp +++ b/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. - ::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]); + blimp.Write_FINO(_amp, _off); } void Fins::output_min() diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index cd4730607f..89cfbb1178 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/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[] = { - PID_TUNING_ROLL, - PID_TUNING_PITCH, - PID_TUNING_YAW, - PID_TUNING_ACCZ + if(blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) { + //No PIDs are used in Manual or Land mode. + return; + } + + 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; iget_rate_roll_pid().get_pid_info(); - // break; - // case PID_TUNING_PITCH: - // pid_info = &blimp.attitude_control->get_rate_pitch_pid().get_pid_info(); - // break; - // case PID_TUNING_YAW: - // pid_info = &blimp.attitude_control->get_rate_yaw_pid().get_pid_info(); - // break; - // case PID_TUNING_ACCZ: - // pid_info = &blimp.pos_control->get_accel_z_pid().get_pid_info(); - // break; + switch (axes[i]) { + case PID_SEND::VELX: + pid_info = &blimp.pid_vel_xy.get_pid_info_x(); + break; + case PID_SEND::VELY: + pid_info = &blimp.pid_vel_xy.get_pid_info_y(); + break; + case PID_SEND::VELZ: + pid_info = &blimp.pid_vel_z.get_pid_info(); + break; + case PID_SEND::VELYAW: + pid_info = &blimp.pid_vel_yaw.get_pid_info(); + 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; } diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 97b4aedb85..3544020d49 100644 --- a/Blimp/GCS_Mavlink.h +++ b/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, + }; }; diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index cb0cf62dc9..b4c2dad526 100644 --- a/Blimp/Log.cpp +++ b/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 -void Blimp::Log_Write_Control_Tuning() +// Write PID packets +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) {} diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 56a1b7b253..27c6eba39d 100644 --- a/Blimp/Parameters.cpp +++ b/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 - // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ - GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + // @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:VELX,1:VELY,2:VELZ;3:VELYAW;4:POSX,5:POSY;6:POZ;7:POSYAW + 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); } diff --git a/Blimp/defines.h b/Blimp/defines.h index f32388b84f..4b9f333ed3 100644 --- a/Blimp/defines.h +++ b/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)