Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Blimp: Add logging and MAVLink

This commit is contained in:
Michelle Rossouw 2021-06-24 12:26:56 +10:00 committed by Andrew Tridgell
parent cceb612d0c
commit 1a0f44a8ad
8 changed files with 186 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Blimp/Blimp.cpp
View File

@ -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();

5
Blimp/Blimp.h
View File

@ -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);

5
Blimp/Fins.cpp
View File

@ -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()

57
Blimp/GCS_Mavlink.cpp
View File

@ -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; 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?
// case PID_TUNING_ROLL:
// pid_info = &blimp.attitude_control->get_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;
}

12
Blimp/GCS_Mavlink.h
View File

@ -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,
};
};

118
Blimp/Log.cpp
View File

@ -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) {}

9
Blimp/Parameters.cpp
View File

@ -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);
}

8
Blimp/defines.h
View File

@ -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)