2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
|
|
|
|
2013-12-30 07:24:36 -04:00
|
|
|
// Write an attitude packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Attitude(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-01-16 12:01:33 -04:00
|
|
|
Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
|
|
|
|
targets.x = nav_roll_cd;
|
|
|
|
targets.y = nav_pitch_cd;
|
|
|
|
|
2017-04-18 09:52:29 -03:00
|
|
|
if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {
|
|
|
|
// when VTOL active log the copter target yaw
|
|
|
|
targets.z = wrap_360_cd(quadplane.attitude_control->get_att_target_euler_cd().z);
|
|
|
|
} else {
|
|
|
|
//Plane does not have the concept of navyaw. This is a placeholder.
|
|
|
|
targets.z = 0;
|
|
|
|
}
|
|
|
|
|
2017-03-30 20:37:47 -03:00
|
|
|
if (quadplane.tailsitter_active()) {
|
|
|
|
DataFlash.Log_Write_AttitudeView(*quadplane.ahrs_view, targets);
|
|
|
|
} else {
|
|
|
|
DataFlash.Log_Write_Attitude(ahrs, targets);
|
|
|
|
}
|
2017-03-13 03:22:41 -03:00
|
|
|
if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {
|
|
|
|
// log quadplane PIDs separately from fixed wing PIDs
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pid_accel_z.get_pid_info() );
|
|
|
|
}
|
|
|
|
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());
|
|
|
|
DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
|
2014-01-01 21:16:20 -04:00
|
|
|
|
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
2017-08-22 00:15:26 -03:00
|
|
|
DataFlash.Log_Write_EKF(ahrs);
|
2014-01-03 20:51:36 -04:00
|
|
|
DataFlash.Log_Write_AHRS2(ahrs);
|
|
|
|
#endif
|
2015-05-04 03:18:55 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2015-11-16 00:10:42 -04:00
|
|
|
sitl.Log_Write_SIMSTATE(&DataFlash);
|
2013-12-30 19:24:38 -04:00
|
|
|
#endif
|
2015-05-15 01:24:46 -03:00
|
|
|
DataFlash.Log_Write_POS(ahrs);
|
2013-12-30 07:24:36 -04:00
|
|
|
}
|
|
|
|
|
2016-05-01 19:51:13 -03:00
|
|
|
// do logging at loop rate
|
|
|
|
void Plane::Log_Write_Fast(void)
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
Log_Write_Attitude();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-01-01 21:16:20 -04:00
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Performance {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2016-04-21 01:51:29 -03:00
|
|
|
uint16_t num_long;
|
2013-01-26 04:07:21 -04:00
|
|
|
uint16_t main_loop_count;
|
2013-10-11 23:30:27 -03:00
|
|
|
uint32_t g_dt_max;
|
2016-04-21 01:51:29 -03:00
|
|
|
uint32_t g_dt_min;
|
2016-04-21 03:11:48 -03:00
|
|
|
uint32_t log_dropped;
|
2017-03-26 18:12:11 -03:00
|
|
|
uint32_t mem_avail;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2013-01-12 07:18:58 -04:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
// Write a performance monitoring packet. Total length : 19 bytes
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Performance()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Performance pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2016-04-21 01:51:29 -03:00
|
|
|
num_long : perf.num_long,
|
2016-04-20 22:50:13 -03:00
|
|
|
main_loop_count : perf.mainLoop_count,
|
|
|
|
g_dt_max : perf.G_Dt_max,
|
2016-04-21 03:11:48 -03:00
|
|
|
g_dt_min : perf.G_Dt_min,
|
2017-03-26 18:12:11 -03:00
|
|
|
log_dropped : DataFlash.num_dropped() - perf.last_log_dropped,
|
|
|
|
hal.util->available_memory()
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2012-08-21 23:19:50 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Startup {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-01-26 04:07:21 -04:00
|
|
|
uint8_t startup_type;
|
2014-03-10 05:20:08 -03:00
|
|
|
uint16_t command_total;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
|
2015-11-09 18:40:34 -04:00
|
|
|
void Plane::Log_Write_Startup(uint8_t type)
|
2013-01-26 04:07:21 -04:00
|
|
|
{
|
|
|
|
struct log_Startup pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-01-26 04:07:21 -04:00
|
|
|
startup_type : type,
|
2014-03-03 09:48:49 -04:00
|
|
|
command_total : mission.num_commands()
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Control_Tuning {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-01-26 04:07:21 -04:00
|
|
|
int16_t nav_roll_cd;
|
|
|
|
int16_t roll;
|
|
|
|
int16_t nav_pitch_cd;
|
|
|
|
int16_t pitch;
|
|
|
|
int16_t throttle_out;
|
|
|
|
int16_t rudder_out;
|
2016-01-30 02:38:31 -04:00
|
|
|
int16_t throttle_dem;
|
2017-10-31 19:16:59 -03:00
|
|
|
float airspeed_estimate;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
// Write a control tuning packet. Total length : 22 bytes
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Control_Tuning()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-10-31 19:16:59 -03:00
|
|
|
float est_airspeed = 0;
|
|
|
|
ahrs.airspeed_estimate(&est_airspeed);
|
|
|
|
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Control_Tuning pkt = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-01-26 04:07:21 -04:00
|
|
|
nav_roll_cd : (int16_t)nav_roll_cd,
|
|
|
|
roll : (int16_t)ahrs.roll_sensor,
|
|
|
|
nav_pitch_cd : (int16_t)nav_pitch_cd,
|
|
|
|
pitch : (int16_t)ahrs.pitch_sensor,
|
2016-10-22 07:27:57 -03:00
|
|
|
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
|
|
|
|
rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
|
2017-10-31 19:16:59 -03:00
|
|
|
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(),
|
|
|
|
airspeed_estimate : est_airspeed
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-04-19 10:37:21 -03:00
|
|
|
struct PACKED log_Nav_Tuning {
|
2013-01-26 04:07:21 -04:00
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2015-01-01 00:17:45 -04:00
|
|
|
float wp_distance;
|
2014-05-21 07:14:00 -03:00
|
|
|
int16_t target_bearing_cd;
|
|
|
|
int16_t nav_bearing_cd;
|
2013-01-26 04:07:21 -04:00
|
|
|
int16_t altitude_error_cm;
|
2016-01-05 19:33:49 -04:00
|
|
|
float xtrack_error;
|
2016-04-20 19:35:03 -03:00
|
|
|
float xtrack_error_i;
|
2016-05-10 16:51:36 -03:00
|
|
|
float airspeed_error;
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
|
2016-01-05 19:33:49 -04:00
|
|
|
// Write a navigation tuning packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Nav_Tuning()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-01-26 04:07:21 -04:00
|
|
|
struct log_Nav_Tuning pkt = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2015-01-01 00:17:45 -04:00
|
|
|
wp_distance : auto_state.wp_distance,
|
2014-05-21 07:14:00 -03:00
|
|
|
target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
|
|
|
|
nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(),
|
2013-01-26 04:07:21 -04:00
|
|
|
altitude_error_cm : (int16_t)altitude_error_cm,
|
2016-04-20 19:35:03 -03:00
|
|
|
xtrack_error : nav_controller->crosstrack_error(),
|
2016-05-10 16:51:36 -03:00
|
|
|
xtrack_error_i : nav_controller->crosstrack_error_integrator(),
|
|
|
|
airspeed_error : airspeed_error
|
2013-01-26 04:07:21 -04:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-04-09 22:42:48 -03:00
|
|
|
struct PACKED log_Status {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2015-04-09 22:42:48 -03:00
|
|
|
uint8_t is_flying;
|
|
|
|
float is_flying_probability;
|
2015-04-29 22:15:08 -03:00
|
|
|
uint8_t armed;
|
|
|
|
uint8_t safety;
|
2015-06-03 16:22:24 -03:00
|
|
|
bool is_crashed;
|
|
|
|
bool is_still;
|
2015-09-25 22:30:26 -03:00
|
|
|
uint8_t stage;
|
2015-11-17 20:11:21 -04:00
|
|
|
bool impact;
|
2015-04-09 22:42:48 -03:00
|
|
|
};
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Status()
|
2015-04-09 22:42:48 -03:00
|
|
|
{
|
|
|
|
struct log_Status pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
|
2015-11-19 23:04:52 -04:00
|
|
|
,time_us : AP_HAL::micros64()
|
2015-04-09 22:42:48 -03:00
|
|
|
,is_flying : is_flying()
|
|
|
|
,is_flying_probability : isFlyingProbability
|
2015-04-29 22:15:08 -03:00
|
|
|
,armed : hal.util->get_soft_armed()
|
2016-02-26 01:12:34 -04:00
|
|
|
,safety : static_cast<uint8_t>(hal.util->safety_switch_state())
|
2015-08-20 17:44:32 -03:00
|
|
|
,is_crashed : crash_state.is_crashed
|
2015-06-03 16:22:24 -03:00
|
|
|
,is_still : plane.ins.is_still()
|
2016-02-26 01:12:34 -04:00
|
|
|
,stage : static_cast<uint8_t>(flight_stage)
|
2015-11-17 20:11:21 -04:00
|
|
|
,impact : crash_state.impact_detected
|
2015-06-03 16:22:24 -03:00
|
|
|
};
|
2015-04-09 22:42:48 -03:00
|
|
|
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2013-10-30 19:23:21 -03:00
|
|
|
struct PACKED log_Sonar {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2016-05-17 22:34:39 -03:00
|
|
|
float distance;
|
2013-10-30 19:23:21 -03:00
|
|
|
float voltage;
|
2014-08-26 08:17:47 -03:00
|
|
|
uint8_t count;
|
2014-08-27 05:25:17 -03:00
|
|
|
float correction;
|
2013-10-30 19:23:21 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// Write a sonar packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Sonar()
|
2013-10-30 19:23:21 -03:00
|
|
|
{
|
2015-05-27 20:51:29 -03:00
|
|
|
uint16_t distance = 0;
|
2017-02-09 07:39:58 -04:00
|
|
|
if (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) {
|
|
|
|
distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
|
2015-05-27 20:51:29 -03:00
|
|
|
}
|
|
|
|
|
2013-10-30 19:23:21 -03:00
|
|
|
struct log_Sonar pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2016-05-17 22:34:39 -03:00
|
|
|
distance : (float)distance*0.01f,
|
2017-02-09 07:39:58 -04:00
|
|
|
voltage : rangefinder.voltage_mv_orient(ROTATION_PITCH_270)*0.001f,
|
2014-08-26 08:17:47 -03:00
|
|
|
count : rangefinder_state.in_range_count,
|
2014-08-27 05:25:17 -03:00
|
|
|
correction : rangefinder_state.correction
|
2013-10-30 19:23:21 -03:00
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
2015-09-10 07:26:48 -03:00
|
|
|
|
|
|
|
DataFlash.Log_Write_RFND(rangefinder);
|
2013-10-30 19:23:21 -03:00
|
|
|
}
|
|
|
|
|
2014-11-02 02:26:05 -04:00
|
|
|
struct PACKED log_Optflow {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2014-11-02 02:26:05 -04:00
|
|
|
uint8_t surface_quality;
|
|
|
|
float flow_x;
|
|
|
|
float flow_y;
|
|
|
|
float body_x;
|
|
|
|
float body_y;
|
|
|
|
};
|
|
|
|
|
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
// Write an optical flow packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Optflow()
|
2014-11-02 02:26:05 -04:00
|
|
|
{
|
|
|
|
// exit immediately if not enabled
|
|
|
|
if (!optflow.enabled()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
const Vector2f &flowRate = optflow.flowRate();
|
|
|
|
const Vector2f &bodyRate = optflow.bodyRate();
|
|
|
|
struct log_Optflow pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2014-11-02 02:26:05 -04:00
|
|
|
surface_quality : optflow.quality(),
|
|
|
|
flow_x : flowRate.x,
|
|
|
|
flow_y : flowRate.y,
|
|
|
|
body_x : bodyRate.x,
|
|
|
|
body_y : bodyRate.y
|
|
|
|
};
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2013-11-27 22:19:34 -04:00
|
|
|
struct PACKED log_Arm_Disarm {
|
|
|
|
LOG_PACKET_HEADER;
|
2015-04-30 00:40:51 -03:00
|
|
|
uint64_t time_us;
|
2013-11-27 22:19:34 -04:00
|
|
|
uint8_t arm_state;
|
|
|
|
uint16_t arm_checks;
|
|
|
|
};
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Current()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2016-06-02 18:00:01 -03:00
|
|
|
DataFlash.Log_Write_Current(battery);
|
2014-02-13 07:08:25 -04:00
|
|
|
|
|
|
|
// also write power status
|
|
|
|
DataFlash.Log_Write_Power();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Arm_Disarm() {
|
2013-11-27 22:19:34 -04:00
|
|
|
struct log_Arm_Disarm pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
|
2015-11-19 23:04:52 -04:00
|
|
|
time_us : AP_HAL::micros64(),
|
2013-11-27 22:19:34 -04:00
|
|
|
arm_state : arming.is_armed(),
|
|
|
|
arm_checks : arming.get_enabled_checks()
|
|
|
|
};
|
2016-04-21 03:11:48 -03:00
|
|
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
2013-11-27 22:19:34 -04:00
|
|
|
}
|
2013-04-19 10:54:40 -03:00
|
|
|
|
2017-07-06 02:56:13 -03:00
|
|
|
|
|
|
|
struct PACKED log_AETR {
|
|
|
|
LOG_PACKET_HEADER;
|
|
|
|
uint64_t time_us;
|
|
|
|
int16_t aileron;
|
|
|
|
int16_t elevator;
|
|
|
|
int16_t throttle;
|
|
|
|
int16_t rudder;
|
|
|
|
int16_t flap;
|
|
|
|
};
|
|
|
|
|
|
|
|
void Plane::Log_Write_AETR()
|
|
|
|
{
|
|
|
|
struct log_AETR pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_AETR_MSG)
|
|
|
|
,time_us : AP_HAL::micros64()
|
|
|
|
,aileron : SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)
|
|
|
|
,elevator : SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)
|
|
|
|
,throttle : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)
|
|
|
|
,rudder : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder)
|
|
|
|
,flap : SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)
|
|
|
|
};
|
|
|
|
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_GPS(uint8_t instance)
|
2013-04-19 18:24:22 -03:00
|
|
|
{
|
2016-05-04 05:32:02 -03:00
|
|
|
if (!ahrs.have_ekf_logging()) {
|
|
|
|
DataFlash.Log_Write_GPS(gps, instance);
|
|
|
|
}
|
2013-04-19 18:24:22 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_IMU()
|
2013-04-19 18:24:22 -03:00
|
|
|
{
|
2013-11-03 23:37:32 -04:00
|
|
|
DataFlash.Log_Write_IMU(ins);
|
2013-04-19 18:24:22 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_RC(void)
|
2013-11-25 17:44:17 -04:00
|
|
|
{
|
|
|
|
DataFlash.Log_Write_RCIN();
|
2013-11-27 07:14:57 -04:00
|
|
|
DataFlash.Log_Write_RCOUT();
|
2015-09-04 12:51:03 -03:00
|
|
|
if (rssi.enabled()) {
|
|
|
|
DataFlash.Log_Write_RSSI(rssi);
|
|
|
|
}
|
2017-07-06 02:56:13 -03:00
|
|
|
Log_Write_AETR();
|
2013-11-25 17:44:17 -04:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Baro(void)
|
2014-01-27 19:35:59 -04:00
|
|
|
{
|
2016-05-04 05:32:02 -03:00
|
|
|
if (!ahrs.have_ekf_logging()) {
|
|
|
|
DataFlash.Log_Write_Baro(barometer);
|
|
|
|
}
|
2014-01-27 19:35:59 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write a AIRSPEED packet
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::Log_Write_Airspeed(void)
|
2014-01-27 19:35:59 -04:00
|
|
|
{
|
2015-01-19 18:10:57 -04:00
|
|
|
DataFlash.Log_Write_Airspeed(airspeed);
|
2014-01-27 19:35:59 -04:00
|
|
|
}
|
|
|
|
|
2017-04-09 08:16:50 -03:00
|
|
|
// Write a AOA and SSA packet
|
|
|
|
void Plane::Log_Write_AOA_SSA(void)
|
|
|
|
{
|
|
|
|
DataFlash.Log_Write_AOA_SSA(ahrs);
|
|
|
|
}
|
|
|
|
|
2015-07-05 23:01:17 -03:00
|
|
|
// log ahrs home and EKF origin to dataflash
|
|
|
|
void Plane::Log_Write_Home_And_Origin()
|
|
|
|
{
|
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
|
|
// log ekf origin if set
|
|
|
|
Location ekf_orig;
|
2016-03-03 03:00:10 -04:00
|
|
|
if (ahrs.get_origin(ekf_orig)) {
|
2015-07-05 23:01:17 -03:00
|
|
|
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
// log ahrs home if set
|
|
|
|
if (home_is_set != HOME_UNSET) {
|
|
|
|
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-12-07 22:54:12 -04:00
|
|
|
// type and unit information can be found in
|
|
|
|
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
|
|
|
// units and "Format characters" for field type information
|
2016-04-16 07:26:43 -03:00
|
|
|
const struct LogStructure Plane::log_structure[] = {
|
2013-04-19 10:37:21 -03:00
|
|
|
LOG_COMMON_STRUCTURES,
|
|
|
|
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
|
2015-12-07 22:54:12 -04:00
|
|
|
"PM", "QHHIIII", "TimeUS,NLon,NLoop,MaxT,MinT,LogDrop,Mem", "ss----b", "FC----0" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_STARTUP_MSG, sizeof(log_Startup),
|
2015-12-07 22:54:12 -04:00
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot", "s--", "F--" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
|
2015-12-07 22:54:12 -04:00
|
|
|
"CTUN", "Qcccchhhf", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,ThrDem,Aspd", "sdddd---n", "FBBBB---0" },
|
2013-04-19 10:37:21 -03:00
|
|
|
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
|
2015-12-07 22:54:12 -04:00
|
|
|
"NTUN", "Qfcccfff", "TimeUS,WpDist,TargBrg,NavBrg,AltErr,XT,XTi,ArspdErr", "smddmmmn", "F0BBB0B0" },
|
2013-10-30 19:23:21 -03:00
|
|
|
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
2015-12-07 22:54:12 -04:00
|
|
|
"SONR", "QffBf", "TimeUS,Dist,Volt,Cnt,Corr", "smv--", "FB0--" },
|
2013-11-27 22:19:34 -04:00
|
|
|
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
|
2015-12-07 22:54:12 -04:00
|
|
|
"ARM", "QBH", "TimeUS,ArmState,ArmChecks", "s--", "F--" },
|
2014-04-30 08:22:16 -03:00
|
|
|
{ LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
|
2015-12-07 22:54:12 -04:00
|
|
|
"ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P", "s---dd-", "F---00-" },
|
2015-04-09 22:42:48 -03:00
|
|
|
{ LOG_STATUS_MSG, sizeof(log_Status),
|
2015-12-07 22:54:12 -04:00
|
|
|
"STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit", "s--------", "F--------" },
|
2016-03-24 22:33:19 -03:00
|
|
|
{ LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
|
2015-12-07 22:54:12 -04:00
|
|
|
"QTUN", "Qffffhhfffff", "TimeUS,AngBst,ThrOut,DAlt,Alt,DCRt,CRt,DVx,DVy,DAx,DAy,TMix", "s--mmnnnnoo-", "F--BBBB0000-" },
|
2017-04-09 08:16:50 -03:00
|
|
|
{ LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA),
|
2015-12-07 22:54:12 -04:00
|
|
|
"AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" },
|
2014-11-02 02:26:05 -04:00
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
|
2015-12-07 22:54:12 -04:00
|
|
|
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
|
2014-11-02 02:26:05 -04:00
|
|
|
#endif
|
2017-03-13 03:22:41 -03:00
|
|
|
{ LOG_PIQR_MSG, sizeof(log_PID), \
|
2015-12-07 22:54:12 -04:00
|
|
|
"PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
2017-03-13 03:22:41 -03:00
|
|
|
{ LOG_PIQP_MSG, sizeof(log_PID), \
|
2015-12-07 22:54:12 -04:00
|
|
|
"PIQP", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
2017-03-13 03:22:41 -03:00
|
|
|
{ LOG_PIQY_MSG, sizeof(log_PID), \
|
2015-12-07 22:54:12 -04:00
|
|
|
"PIQY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
2017-03-13 03:22:41 -03:00
|
|
|
{ LOG_PIQA_MSG, sizeof(log_PID), \
|
2015-12-07 22:54:12 -04:00
|
|
|
"PIQA", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
2017-07-06 02:56:13 -03:00
|
|
|
{ LOG_AETR_MSG, sizeof(log_AETR), \
|
2015-12-07 22:54:12 -04:00
|
|
|
"AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap", "s-----", "F-----" }, \
|
2013-04-19 10:37:21 -03:00
|
|
|
};
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-08-06 10:25:22 -03:00
|
|
|
void Plane::Log_Write_Vehicle_Startup_Messages()
|
|
|
|
{
|
|
|
|
// only 200(?) bytes are guaranteed by DataFlash
|
|
|
|
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
2017-10-23 21:39:45 -03:00
|
|
|
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
2016-07-02 03:55:01 -03:00
|
|
|
DataFlash.Log_Write_Rally(rally);
|
2017-03-13 19:43:16 -03:00
|
|
|
Log_Write_Home_And_Origin();
|
2016-08-01 09:39:47 -03:00
|
|
|
gps.Write_DataFlash_Log_Startup_messages();
|
2015-08-06 10:25:22 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
/*
|
|
|
|
initialise logging subsystem
|
|
|
|
*/
|
|
|
|
void Plane::log_init(void)
|
|
|
|
{
|
2015-07-06 12:30:40 -03:00
|
|
|
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
#else // LOGGING_ENABLED
|
2013-01-12 07:18:58 -04:00
|
|
|
|
2015-07-31 18:26:14 -03:00
|
|
|
void Plane::Log_Write_Attitude(void) {}
|
2017-01-01 23:54:24 -04:00
|
|
|
void Plane::Log_Write_Fast(void) {}
|
2015-07-31 18:26:14 -03:00
|
|
|
void Plane::Log_Write_Performance() {}
|
|
|
|
void Plane::Log_Write_Startup(uint8_t type) {}
|
|
|
|
void Plane::Log_Write_Control_Tuning() {}
|
|
|
|
void Plane::Log_Write_Nav_Tuning() {}
|
|
|
|
void Plane::Log_Write_Status() {}
|
|
|
|
void Plane::Log_Write_Sonar() {}
|
|
|
|
|
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
void Plane::Log_Write_Optflow() {}
|
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-07-31 18:26:14 -03:00
|
|
|
void Plane::Log_Write_Current() {}
|
|
|
|
void Plane::Log_Arm_Disarm() {}
|
|
|
|
void Plane::Log_Write_GPS(uint8_t instance) {}
|
|
|
|
void Plane::Log_Write_IMU() {}
|
|
|
|
void Plane::Log_Write_RC(void) {}
|
|
|
|
void Plane::Log_Write_Baro(void) {}
|
|
|
|
void Plane::Log_Write_Airspeed(void) {}
|
|
|
|
void Plane::Log_Write_Home_And_Origin() {}
|
|
|
|
|
|
|
|
void Plane::log_init(void) {}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
#endif // LOGGING_ENABLED
|