|
|
|
@ -2321,21 +2321,25 @@ bool QuadPlane::verify_vtol_land(void)
@@ -2321,21 +2321,25 @@ bool QuadPlane::verify_vtol_land(void)
|
|
|
|
|
// Write a control tuning packet
|
|
|
|
|
void QuadPlane::Log_Write_QControl_Tuning() |
|
|
|
|
{ |
|
|
|
|
const Vector3f &desired_velocity = pos_control->get_desired_velocity(); |
|
|
|
|
const Vector3f &accel_target = pos_control->get_accel_target(); |
|
|
|
|
float des_alt_m = 0.0f; |
|
|
|
|
int16_t target_climb_rate_cms = 0; |
|
|
|
|
if (plane.control_mode != QSTABILIZE) { |
|
|
|
|
des_alt_m = pos_control->get_alt_target() / 100.0f; |
|
|
|
|
target_climb_rate_cms = pos_control->get_vel_target_z(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct log_QControl_Tuning pkt = { |
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG), |
|
|
|
|
time_us : AP_HAL::micros64(), |
|
|
|
|
throttle_in : attitude_control->get_throttle_in(), |
|
|
|
|
angle_boost : attitude_control->angle_boost(), |
|
|
|
|
throttle_out : motors->get_throttle(), |
|
|
|
|
desired_alt : pos_control->get_alt_target() / 100.0f, |
|
|
|
|
throttle_hover : motors->get_throttle_hover(), |
|
|
|
|
desired_alt : des_alt_m, |
|
|
|
|
inav_alt : inertial_nav.get_altitude() / 100.0f, |
|
|
|
|
desired_climb_rate : (int16_t)pos_control->get_vel_target_z(), |
|
|
|
|
climb_rate : (int16_t)inertial_nav.get_velocity_z(), |
|
|
|
|
dvx : desired_velocity.x*0.01f, |
|
|
|
|
dvy : desired_velocity.y*0.01f, |
|
|
|
|
dax : accel_target.x*0.01f, |
|
|
|
|
day : accel_target.y*0.01f, |
|
|
|
|
baro_alt : int32_t(plane.barometer.get_altitude() * 100), |
|
|
|
|
target_climb_rate : target_climb_rate_cms, |
|
|
|
|
climb_rate : int16_t(inertial_nav.get_velocity_z()), |
|
|
|
|
throttle_mix : attitude_control->get_throttle_mix(), |
|
|
|
|
}; |
|
|
|
|
plane.DataFlash.WriteBlock(&pkt, sizeof(pkt)); |
|
|
|
|