void SoloGimbal::write_logs()
{
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
uint32_t tstamp = AP_HAL::millis();
Vector3f eulerEst;
Quaternion quatEst;
_ekf.getQuat(quatEst);
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
struct log_Gimbal1 pkt1 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
time_ms : tstamp,
delta_time : _log_dt,
delta_angles_x : _log_del_ang.x,
delta_angles_y : _log_del_ang.y,
delta_angles_z : _log_del_ang.z,
delta_velocity_x : _log_del_vel.x,
delta_velocity_y : _log_del_vel.y,
delta_velocity_z : _log_del_vel.z,
joint_angles_x : _measurement.joint_angles.x,
joint_angles_y : _measurement.joint_angles.y,
joint_angles_z : _measurement.joint_angles.z
};
logger->WriteBlock(&pkt1, sizeof(pkt1));
struct log_Gimbal2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
time_ms : tstamp,
est_sta : (uint8_t) _ekf.getStatus(),
est_x : eulerEst.x,
est_y : eulerEst.y,
est_z : eulerEst.z,
rate_x : _ang_vel_dem_rads.x,
rate_y : _ang_vel_dem_rads.y,
rate_z : _ang_vel_dem_rads.z,
target_x: _att_target_euler_rad.x,
target_y: _att_target_euler_rad.y,
target_z: _att_target_euler_rad.z
};
logger->WriteBlock(&pkt2, sizeof(pkt2));
_log_dt = 0;
_log_del_ang.zero();
_log_del_vel.zero();
}
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 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));
}