diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 10e9f0484b..641f45b657 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -370,7 +370,12 @@ private: void tiltrotor_update(void); void tiltrotor_continuous_update(void); void tiltrotor_binary_update(void); + void tilt_compensate_up(float *thrust, uint8_t num_motors); + void tilt_compensate_down(float *thrust, uint8_t num_motors); void tilt_compensate(float *thrust, uint8_t num_motors); + bool is_motor_tilting(uint8_t motor) const { + return (((uint8_t)tilt.tilt_mask.get()) & (1U< 0.98f) { - tilt_factor = 1.0 / cosf(radians(0.98f*90)); + inv_tilt_factor = 1.0 / cosf(radians(0.98f*90)); } else { - tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90)); + inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90)); } // when we got past Q_TILT_MAX we gang the tilted motors together @@ -218,12 +227,11 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors) float tilt_total = 0; uint8_t tilt_count = 0; - uint8_t mask = tilt.tilt_mask; - // apply _tilt_factor first + // apply inv_tilt_factor first for (uint8_t i=0; i tilt_threshold); + + float tilt_total = 0; + uint8_t tilt_count = 0; + + // apply tilt_factor first + for (uint8_t i=0; i