int write_version(int fd)
{
/* construct version message */
struct {
LOG_PACKET_HEADER;
struct log_VER_s body;
} log_msg_VER = {
LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
};
/* fill version message and write it */
strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
return write(fd, &log_msg_VER, sizeof(log_msg_VER));
}
// Private Methods /////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (!should_df_log()) {
return;
}
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)),
time_us : AP_HAL::micros64(),
instance : state.instance,
noisePerMS : _buffer.mon_hw_60.noisePerMS,
jamInd : _buffer.mon_hw_60.jamInd,
aPower : _buffer.mon_hw_60.aPower,
agcCnt : _buffer.mon_hw_60.agcCnt,
config : _unconfigured_messages,
};
void
AP_GPS_SBP::logging_log_full_update()
{
if (!should_df_log()) {
return;
}
struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
time_us : AP_HAL::micros64(),
crc_error_counter : crc_error_counter,
last_injected_data_ms : last_injected_data_ms,
last_iar_num_hypotheses : last_iar_num_hypotheses,
};
// Write an Autotune data packet
void Copter::ModeAutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
{
struct log_AutoTune pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
time_us : AP_HAL::micros64(),
axis : _axis,
tune_step : tune_step,
meas_target : meas_target,
meas_min : meas_min,
meas_max : meas_max,
new_gain_rp : new_gain_rp,
new_gain_rd : new_gain_rd,
new_gain_sp : new_gain_sp,
new_ddt : new_ddt
};
// Write a navigation tuning packe
void Plane::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_us : hal.scheduler->micros64(),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : auto_state.wp_distance,
target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(),
altitude_error_cm : (int16_t)altitude_error_cm,
airspeed_cm : (int16_t)airspeed.get_airspeed_cm(),
altitude : barometer.get_altitude(),
groundspeed_cm : (uint32_t)(gps.ground_speed()*100)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// 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 : hal.scheduler->micros64(),
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,
throttle_out : (int16_t)channel_throttle->servo_out,
rudder_out : (int16_t)channel_rudder->servo_out,
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a throttle control packet
void Rover::Log_Write_Throttle()
{
const Vector3f accel = ins.get_accel();
float speed = DataFlash.quiet_nanf();
g2.attitude_control.get_forward_speed(speed);
struct log_Throttle pkt = {
LOG_PACKET_HEADER_INIT(LOG_THR_MSG),
time_us : AP_HAL::micros64(),
throttle_in : channel_throttle->get_control_in(),
throttle_out : g2.motors.get_throttle(),
desired_speed : g2.attitude_control.get_desired_speed(),
speed : speed,
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a steering packet
void Rover::Log_Write_Steering()
{
float lat_accel = DataFlash.quiet_nanf();
g2.attitude_control.get_lat_accel(lat_accel);
struct log_Steering pkt = {
LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
time_us : AP_HAL::micros64(),
steering_in : channel_steer->get_control_in(),
steering_out : g2.motors.get_steering(),
desired_lat_accel : g2.attitude_control.get_desired_lat_accel(),
lat_accel : lat_accel,
desired_turn_rate : degrees(g2.attitude_control.get_desired_turn_rate()),
turn_rate : degrees(ahrs.get_yaw_rate_earth())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a performance monitoring packet. Total length : 19 bytes
void Plane::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : hal.scheduler->micros64(),
loop_time : millis() - perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000),
gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000),
i2c_lockup_count: hal.i2c->lockup_count(),
ins_error_count : ins.error_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a control tuning packet. Total length : 22 bytes
void Plane::Log_Write_Control_Tuning()
{
float est_airspeed = 0;
ahrs.airspeed_estimate(&est_airspeed);
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
time_us : AP_HAL::micros64(),
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,
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),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(),
airspeed_estimate : est_airspeed
};
void AP_GPS_UBLOX::log_mon_hw2(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX2_MSG)),
time_us : AP_HAL::micros64(),
instance : state.instance,
ofsI : _buffer.mon_hw2.ofsI,
magI : _buffer.mon_hw2.magI,
ofsQ : _buffer.mon_hw2.ofsQ,
magQ : _buffer.mon_hw2.magQ,
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_UBLOX::log_mon_hw2(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(LOG_UBX2_MSG),
timestamp : hal.scheduler->millis(),
instance : state.instance,
ofsI : _buffer.mon_hw2.ofsI,
magI : _buffer.mon_hw2.magI,
ofsQ : _buffer.mon_hw2.ofsQ,
magQ : _buffer.mon_hw2.magQ,
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
void
AP_GPS_SBP::logging_log_full_update()
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
timestamp : hal.scheduler->millis(),
crc_error_counter : crc_error_counter,
last_injected_data_ms : last_injected_data_ms,
last_iar_num_hypotheses : last_iar_num_hypotheses,
};
// Write a performance monitoring packet. Total length : 19 bytes
void Rover::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : AP_HAL::micros64(),
loop_time : millis()- perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000),
gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000),
i2c_lockup_count: 0,
ins_error_count : ins.error_count(),
hal.util->available_memory()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void DataFlashTest_AllTypes::Log_Write_TypeMessages()
{
dataflash.StartNewLog();
log_num = dataflash.find_last_log();
hal.console->printf("Using log number %u\n", log_num);
struct log_TYP1 typ1 = {
LOG_PACKET_HEADER_INIT(LOG_TYP1_MSG),
time_us : AP_HAL::micros64(),
b : -17, // int8_t
B : 42, // uint8_t
h : -12372, // int16_t
H : 19812, // uint16_t
i : -98234729, // int32_t
I : 74627293, // uint32_t
f : 35.87654, // float
d : 67.7393274658293, // double
n : { 'A', 'B', 'C', 'D' }, // char[4]
void AP_AutoTune::write_log(float servo, float demanded, float achieved)
{
if (!dataflash.logging_started()) {
return;
}
struct log_ATRP pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
time_us : AP_HAL::micros64(),
type : type,
state : (uint8_t)state,
servo : (int16_t)(servo*100),
demanded : demanded,
achieved : achieved,
P : current.P.get()
};
dataflash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a sonar packet
void Rover::Log_Write_Sonar()
{
uint16_t turn_time = 0;
if (!is_zero(obstacle.turn_angle)) {
turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : lateral_acceleration,
sonar1_distance : sonar.distance_cm(0),
sonar2_distance : sonar.distance_cm(1),
detected_count : obstacle.detected_count,
turn_angle : static_cast<int8_t>(obstacle.turn_angle),
turn_time : turn_time,
ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100)),
throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))
};
void AP_AutoTune::write_log(float servo, float demanded, float achieved)
{
AP_Logger *dataflash = AP_Logger::get_singleton();
if (!dataflash->logging_started()) {
return;
}
struct log_ATRP pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
time_us : AP_HAL::micros64(),
type : static_cast<uint8_t>(type),
state : (uint8_t)state,
servo : (int16_t)(servo*100),
demanded : demanded,
achieved : achieved,
P : current.P.get()
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
void
AP_GPS_SBP2::logging_log_full_update()
{
if (!should_df_log()) {
return;
}
//TODO: Expand with heartbeat info.
//TODO: Get rid of IAR NUM HYPO
struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
time_us : AP_HAL::micros64(),
crc_error_counter : crc_error_counter,
last_injected_data_ms : last_injected_data_ms,
last_iar_num_hypotheses : 0,
};
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
};
void write_formats(int fd)
{
/* construct message format packet */
struct {
LOG_PACKET_HEADER;
struct log_format_s body;
} log_format_packet = {
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
};
/* fill message format packet for each format and write to log */
int i;
for (i = 0; i < log_formats_num; i++) {
log_format_packet.body = log_formats[i];
log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
}
fsync(fd);
}
// Write a control tuning packet. Total length : 22 bytes
void Rover::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(),
steer_out :
(int16_t)channel_steer->get_servo_out(),
roll :
(int16_t)ahrs.roll_sensor,
pitch :
(int16_t)ahrs.pitch_sensor,
throttle_out :
(int16_t)channel_throttle->get_servo_out(),
accel_y :
accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_UBX1_MSG),
timestamp : hal.scheduler->millis(),
instance : state.instance,
noisePerMS : _buffer.mon_hw_60.noisePerMS,
jamInd : _buffer.mon_hw_60.jamInd,
aPower : _buffer.mon_hw_60.aPower
};
if (_payload_length == 68) {
pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS;
pkt.jamInd = _buffer.mon_hw_68.jamInd;
pkt.aPower = _buffer.mon_hw_68.aPower;
}
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
int write_formats(int fd)
{
/* construct message format packet */
struct {
LOG_PACKET_HEADER;
struct log_format_s body;
} log_msg_format = {
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
};
int written = 0;
/* fill message format packet for each format and write it */
for (int i = 0; i < log_formats_num; i++) {
log_msg_format.body = log_formats[i];
written += write(fd, &log_msg_format, sizeof(log_msg_format));
}
return written;
}
// Write a rangefinder packet
void Rover::Log_Write_Rangefinder()
{
uint16_t turn_time = 0;
if (!is_zero(obstacle.turn_angle)) {
turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
}
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
struct log_Rangefinder pkt = {
LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : g2.attitude_control.get_desired_lat_accel(),
rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0,
rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0,
detected_count : obstacle.detected_count,
turn_angle : static_cast<int8_t>(obstacle.turn_angle),
turn_time : turn_time,
ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100.0f)),
throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))
};
// Write an optical flow packet
void Sub::Log_Write_Optflow()
{
#if OPTFLOW == ENABLED
// 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),
time_us : AP_HAL::micros64(),
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 // OPTFLOW == ENABLED
}
// Write a navigation tuning packet
void Rover::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_us :
AP_HAL::micros64(),
yaw :
(uint16_t)ahrs.yaw_sensor,
wp_distance :
wp_distance,
target_bearing_cd :
(uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd :
(uint16_t)nav_controller->nav_bearing_cd(),
throttle :
(int8_t)(100 * channel_throttle->norm_output()),
xtrack_error :
nav_controller->crosstrack_error()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
void OpticalFlow::Log_Write_Optflow()
{
AP_Logger *instance = AP_Logger::instance();
if (instance == nullptr) {
return;
}
if (_log_bit != (uint32_t)-1 &&
!instance->should_log(_log_bit)) {
return;
}
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_us : AP_HAL::micros64(),
surface_quality : _state.surface_quality,
flow_x : _state.flowRate.x,
flow_y : _state.flowRate.y,
body_x : _state.bodyRate.x,
body_y : _state.bodyRate.y
};
instance->WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)),
time_us : AP_HAL::micros64(),
instance : state.instance,
noisePerMS : _buffer.mon_hw_60.noisePerMS,
jamInd : _buffer.mon_hw_60.jamInd,
aPower : _buffer.mon_hw_60.aPower,
agcCnt : _buffer.mon_hw_60.agcCnt,
};
if (_payload_length == 68) {
pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS;
pkt.jamInd = _buffer.mon_hw_68.jamInd;
pkt.aPower = _buffer.mon_hw_68.aPower;
pkt.agcCnt = _buffer.mon_hw_68.agcCnt;
}
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash)
{
float yaw;
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_us : AP_HAL::micros64(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
