diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index eb031768c4..386e7d1c70 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -205,7 +205,8 @@ void AP_Motors6DOF::output_min() int8_t i; // set limits flags - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; limit.yaw = true; limit.throttle_lower = false; limit.throttle_upper = false; @@ -302,7 +303,8 @@ void AP_Motors6DOF::output_armed_stabilizing() float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor // initialize limits flags - limit.roll_pitch = false; + limit.roll = false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -407,7 +409,8 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored() float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor // initialize limits flags - limit.roll_pitch = false; + limit.roll= false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -493,7 +496,8 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof() float yfl_max; // initialize limits flags - limit.roll_pitch = false; + limit.roll = false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 5740ca4019..dd222c8592 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -163,7 +163,8 @@ void AP_MotorsCoax::output_armed_stabilizing() } else { rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f * (float)_yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f); if (rp_scale < 1.0f) { - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; } } @@ -198,7 +199,8 @@ void AP_MotorsCoax::output_armed_stabilizing() float thrust_out_actuator = constrain_float(MAX(_throttle_hover * 0.5f, thrust_out), 0.5f, 1.0f); if (is_zero(thrust_out)) { - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; } // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // static thrust is proportional to the airflow velocity squared @@ -207,11 +209,11 @@ void AP_MotorsCoax::output_armed_stabilizing() _actuator_out[0] = roll_thrust / thrust_out_actuator; _actuator_out[1] = pitch_thrust / thrust_out_actuator; if (fabsf(_actuator_out[0]) > 1.0f) { - limit.roll_pitch = true; + limit.roll = true; _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); } if (fabsf(_actuator_out[1]) > 1.0f) { - limit.roll_pitch = true; + limit.pitch = true; _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); } _actuator_out[2] = -_actuator_out[0]; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index a442cc4ab6..c6fb1e9c8e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -214,7 +214,8 @@ void AP_MotorsHeli::output_min() update_motor_control(ROTOR_CONTROL_STOP); // override limits flags - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; limit.yaw = true; limit.throttle_lower = true; limit.throttle_upper = false; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 6fc2b24fed..1b429cb8bf 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -374,7 +374,8 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) { // initialize limits flag - limit.roll_pitch = false; + limit.roll = false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -382,22 +383,22 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { if (pitch_out < -_cyclic_max/4500.0f) { pitch_out = -_cyclic_max/4500.0f; - limit.roll_pitch = true; + limit.pitch = true; } if (pitch_out > _cyclic_max/4500.0f) { pitch_out = _cyclic_max/4500.0f; - limit.roll_pitch = true; + limit.pitch = true; } } else { if (roll_out < -_cyclic_max/4500.0f) { roll_out = -_cyclic_max/4500.0f; - limit.roll_pitch = true; + limit.roll = true; } if (roll_out > _cyclic_max/4500.0f) { roll_out = _cyclic_max/4500.0f; - limit.roll_pitch = true; + limit.roll = true; } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 6bb51488ad..4c2b172fe1 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -211,7 +211,8 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state) void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) { // initialize limits flag - limit.roll_pitch = false; + limit.roll = false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 13020c7491..8004361107 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -331,7 +331,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float float yaw_offset = 0.0f; // initialize limits flag - limit.roll_pitch = false; + limit.roll = false; + limit.pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -350,7 +351,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float float ratio = (float)(_cyclic_max/4500.0f) / total_out; roll_out *= ratio; pitch_out *= ratio; - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; } // constrain collective input diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 724b047695..3f1efc7fa0 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -218,7 +218,8 @@ void AP_MotorsMatrix::output_armed_stabilizing() // check for roll and pitch saturation if (rp_high - rp_low > 1.0f || throttle_avg_max < -rp_low) { // Full range is being used by roll and pitch. - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; } // calculate the highest allowed average thrust that will provide maximum control range @@ -275,7 +276,8 @@ void AP_MotorsMatrix::output_armed_stabilizing() thr_adj = throttle_thrust - throttle_thrust_best_rpy; if (rpy_scale < 1.0f) { // Full range is being used by roll, pitch, and yaw. - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; limit.yaw = true; if (thr_adj > 0.0f) { limit.throttle_upper = true; diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp index c9fec2e862..aab1148b4b 100644 --- a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp @@ -104,7 +104,8 @@ void AP_MotorsMatrixTS::output_armed_stabilizing() if (thrust_max > 1.0f) { thr_adj = 1.0f - thrust_max; limit.throttle_upper = true; - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; for (int i=0; i thrust_out_actuator && !is_zero(actuator_max)) { // roll, pitch and yaw request can not be achieved at full servo defection // reduce roll, pitch and yaw to reduce the requested defection to maximum - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; limit.yaw = true; rp_scale = thrust_out_actuator / actuator_max; } else { diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index eb1eead888..0f7a272868 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -161,7 +161,8 @@ void AP_MotorsTailsitter::output_armed_stabilizing() if (thrust_max > 1.0f) { thr_adj = 1.0f - thrust_max; limit.throttle_upper = true; - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; } // Add adjustment to reduce average throttle diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index d681b0246d..9851b10875 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -221,7 +221,8 @@ void AP_MotorsTri::output_armed_stabilizing() thr_adj = throttle_thrust - throttle_thrust_best_rpy; if (rpy_scale < 1.0f) { // Full range is being used by roll, pitch, and yaw. - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; if (thr_adj > 0.0f) { limit.throttle_upper = true; } diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index f6e5db8832..0d6bca2dc3 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -45,7 +45,8 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : _throttle_filter.reset(0.0f); // init limit flags - limit.roll_pitch = true; + limit.roll = true; + limit.pitch = true; limit.yaw = true; limit.throttle_lower = true; limit.throttle_upper = true; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index ee44f8860f..e01131eab7 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -136,7 +136,8 @@ public: // structure for holding motor limit flags struct AP_Motors_limit { - uint8_t roll_pitch : 1; // we have reached roll or pitch limit + uint8_t roll : 1; // we have reached roll or pitch limit + uint8_t pitch : 1; // we have reached roll or pitch limit uint8_t yaw : 1; // we have reached yaw limit uint8_t throttle_lower : 1; // we have reached throttle's lower limit uint8_t throttle_upper : 1; // we have reached throttle's upper limit diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 77c15f2ae5..bc4c8a8a34 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -148,11 +148,11 @@ void stability_test() avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output #if NUM_OUTPUTS <= 4 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad + hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad #elif NUM_OUTPUTS <= 6 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa + hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa #else - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa + hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa #endif (int)roll_in, (int)pitch_in, @@ -171,7 +171,8 @@ void stability_test() (int)hal.rcout->read(7), #endif (int)avg_out, - (int)motors.limit.roll_pitch, + (int)motors.limit.roll, + (int)motors.limit.pitch, (int)motors.limit.yaw, (int)motors.limit.throttle_lower, (int)motors.limit.throttle_upper);