|
|
|
@ -242,13 +242,12 @@ struct PACKED log_Control_Tuning {
@@ -242,13 +242,12 @@ struct PACKED log_Control_Tuning {
|
|
|
|
|
int16_t pitch; |
|
|
|
|
int16_t throttle_out; |
|
|
|
|
int16_t rudder_out; |
|
|
|
|
float accel_y; |
|
|
|
|
int16_t throttle_dem; |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
// Write a control tuning packet. Total length : 22 bytes
|
|
|
|
|
void Plane::Log_Write_Control_Tuning() |
|
|
|
|
{ |
|
|
|
|
Vector3f accel = ins.get_accel(); |
|
|
|
|
struct log_Control_Tuning pkt = { |
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), |
|
|
|
|
time_us : AP_HAL::micros64(), |
|
|
|
@ -258,7 +257,7 @@ void Plane::Log_Write_Control_Tuning()
@@ -258,7 +257,7 @@ void Plane::Log_Write_Control_Tuning()
|
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor, |
|
|
|
|
throttle_out : (int16_t)channel_throttle->servo_out, |
|
|
|
|
rudder_out : (int16_t)channel_rudder->servo_out, |
|
|
|
|
accel_y : accel.y |
|
|
|
|
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand() |
|
|
|
|
}; |
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt)); |
|
|
|
|
} |
|
|
|
@ -340,7 +339,7 @@ struct PACKED log_Sonar {
@@ -340,7 +339,7 @@ struct PACKED log_Sonar {
|
|
|
|
|
float voltage; |
|
|
|
|
float baro_alt; |
|
|
|
|
float groundspeed; |
|
|
|
|
uint8_t throttle; |
|
|
|
|
int8_t throttle; |
|
|
|
|
uint8_t count; |
|
|
|
|
float correction; |
|
|
|
|
}; |
|
|
|
@ -361,7 +360,7 @@ void Plane::Log_Write_Sonar()
@@ -361,7 +360,7 @@ void Plane::Log_Write_Sonar()
|
|
|
|
|
voltage : rangefinder.voltage_mv()*0.001f, |
|
|
|
|
baro_alt : barometer.get_altitude(), |
|
|
|
|
groundspeed : gps.ground_speed(), |
|
|
|
|
throttle : (uint8_t)(100 * channel_throttle->norm_output()), |
|
|
|
|
throttle : (int8_t)(100 * channel_throttle->norm_output()), |
|
|
|
|
count : rangefinder_state.in_range_count, |
|
|
|
|
correction : rangefinder_state.correction |
|
|
|
|
}; |
|
|
|
@ -483,11 +482,11 @@ static const struct LogStructure log_structure[] = {
@@ -483,11 +482,11 @@ static const struct LogStructure log_structure[] = {
|
|
|
|
|
{ LOG_STARTUP_MSG, sizeof(log_Startup),
|
|
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot" }, |
|
|
|
|
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
|
|
|
|
|
"CTUN", "Qcccchhf", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" }, |
|
|
|
|
"CTUN", "Qcccchhh", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,ThrDem" }, |
|
|
|
|
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
|
|
|
|
"NTUN", "QCfccccfIf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM,XT" }, |
|
|
|
|
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
|
|
|
|
"SONR", "QHfffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, |
|
|
|
|
"SONR", "QHfffbBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, |
|
|
|
|
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), |
|
|
|
|
"ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, |
|
|
|
|
{ LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP), |
|
|
|
|