diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 41819238dc..4e2974e6bb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2090,6 +2090,17 @@ void QuadPlane::poscontrol_init_approach(void) } } +/* + log the QPOS message + */ +void QuadPlane::log_QPOS(void) +{ + AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist", "QBf", + AP_HAL::micros64(), + poscontrol.get_state(), + plane.auto_state.wp_distance); +} + /* change position control state */ @@ -2116,6 +2127,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // reset throttle descent control qp.thr_ctrl_land = false; } + qp.log_QPOS(); } state = s; last_state_change_ms = AP_HAL::millis(); @@ -2551,10 +2563,7 @@ void QuadPlane::vtol_position_controller(void) if (now_ms - poscontrol.last_log_ms >= 40) { // log poscontrol at 25Hz poscontrol.last_log_ms = now_ms; - AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist", "QBf", - AP_HAL::micros64(), - poscontrol.get_state(), - plane.auto_state.wp_distance); + log_QPOS(); } } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index eb5a9426e6..61564c2d38 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -249,6 +249,7 @@ private: bool should_relax(void); void motors_output(bool run_rate_controller = true); void Log_Write_QControl_Tuning(); + void log_QPOS(void); float landing_descent_rate_cms(float height_above_ground); // setup correct aux channels for frame class