diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7414348b45..5442984189 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -837,7 +837,7 @@ void QuadPlane::update_transition(void) assisted_flight = true; hold_hover(assist_climb_rate_cms()); attitude_control->rate_controller_run(); - motors->output(); + motors_output(); last_throttle = motors->get_throttle(); break; } @@ -856,7 +856,7 @@ void QuadPlane::update_transition(void) assisted_flight = true; hold_stabilize(throttle_scaled); attitude_control->rate_controller_run(); - motors->output(); + motors_output(); break; } @@ -889,7 +889,7 @@ void QuadPlane::update(void) attitude_control->rate_controller_run(); // output to motors - motors->output(); + motors_output(); transition_start_ms = 0; if (throttle_wait && !plane.is_flying()) { transition_state = TRANSITION_DONE; @@ -908,6 +908,15 @@ void QuadPlane::update(void) } } +/* + output motors and do any copter needed + */ +void QuadPlane::motors_output(void) +{ + motors->output(); + plane.DataFlash.Log_Write_Rate(plane.ahrs, *motors, *attitude_control, *pos_control); +} + /* update control mode for quadplane modes */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index fdf5876679..3186981f46 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -128,7 +128,8 @@ private: float desired_auto_yaw_rate_cds(void); bool should_relax(void); - + void motors_output(void); + // setup correct aux channels for frame class void setup_default_channels(uint8_t num_motors);