// write log to dataflash
void AC_PosControl::write_log()
{
const Vector3f &pos_target = get_pos_target();
const Vector3f &vel_target = get_vel_target();
const Vector3f &accel_target = get_accel_target();
const Vector3f &position = _inav.get_position();
const Vector3f &velocity = _inav.get_velocity();
float accel_x, accel_y;
lean_angles_to_accel(accel_x, accel_y);
DataFlash_Class::instance()->Log_Write("PSC", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
"smmmmnnnnoooo", "FBBBBBBBBBBBB", "Qffffffffffff",
AP_HAL::micros64(),
(double)pos_target.x,
(double)pos_target.y,
(double)position.x,
(double)position.y,
(double)vel_target.x,
(double)vel_target.y,
(double)velocity.x,
(double)velocity.y,
(double)accel_target.x,
(double)accel_target.y,
(double)accel_x,
(double)accel_y);
}
void AC_PosControl::write_log()
{
const Vector3f &pos_target = get_pos_target();
const Vector3f &vel_target = get_vel_target();
const Vector3f &accel_target = get_accel_target();
const Vector3f &position = _inav.get_position();
const Vector3f &velocity = _inav.get_velocity();
float accel_x, accel_y;
lean_angles_to_accel(accel_x, accel_y);
AP::logger().Write("PSC",
"TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
"smmmmnnnnoooo",
"F000000000000",
"Qffffffffffff",
AP_HAL::micros64(),
double(pos_target.x * 0.01f),
double(pos_target.y * 0.01f),
double(position.x * 0.01f),
double(position.y * 0.01f),
double(vel_target.x * 0.01f),
double(vel_target.y * 0.01f),
double(velocity.x * 0.01f),
double(velocity.y * 0.01f),
double(accel_target.x * 0.01f),
double(accel_target.y * 0.01f),
double(accel_x * 0.01f),
double(accel_y * 0.01f));
}
