|
|
|
@ -526,16 +526,16 @@ bool QuadPlane::setup(void)
@@ -526,16 +526,16 @@ bool QuadPlane::setup(void)
|
|
|
|
|
|
|
|
|
|
switch (motor_class) { |
|
|
|
|
case AP_Motors::MOTOR_FRAME_TRI: |
|
|
|
|
motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz()); |
|
|
|
|
motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed); |
|
|
|
|
motors_var_info = AP_MotorsTri::var_info; |
|
|
|
|
break; |
|
|
|
|
case AP_Motors::MOTOR_FRAME_TAILSITTER: |
|
|
|
|
motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz()); |
|
|
|
|
motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); |
|
|
|
|
motors_var_info = AP_MotorsTailsitter::var_info; |
|
|
|
|
rotation = ROTATION_PITCH_90; |
|
|
|
|
break; |
|
|
|
|
default: |
|
|
|
|
motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz()); |
|
|
|
|
motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed); |
|
|
|
|
motors_var_info = AP_MotorsMatrix::var_info; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -1170,7 +1170,7 @@ bool QuadPlane::assistance_needed(float aspeed)
@@ -1170,7 +1170,7 @@ bool QuadPlane::assistance_needed(float aspeed)
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint32_t max_angle_cd = 100U*assist_angle; |
|
|
|
|
int32_t max_angle_cd = 100U*assist_angle; |
|
|
|
|
if ((labs(ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd && |
|
|
|
|
labs(ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd)) { |
|
|
|
|
// not beyond angle error
|
|
|
|
|