From ad00e0ee1e7e7b88f453f43da223f13090b7c508 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 14 May 2013 18:03:34 +0900 Subject: [PATCH] ACMotors: make test_order uint_8 --- libraries/AP_HAL_AVR/AnalogIn_ADC.cpp | 8 +++---- libraries/AP_Motors/AP_MotorsMatrix.cpp | 28 +++++++++++-------------- libraries/AP_Motors/AP_MotorsMatrix.h | 11 +++------- 3 files changed, 19 insertions(+), 28 deletions(-) diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 442dd98f87..212b0a47f9 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -12,12 +12,12 @@ using namespace AP_HAL_AVR; extern const AP_HAL::HAL& hal; ADCSource::ADCSource(uint8_t pin) : - _pin(pin), - _stop_pin(ANALOG_INPUT_NONE), _sum_count(0), _sum(0), - _settle_time_ms(0), - _last_average(0) + _last_average(0), + _pin(pin), + _stop_pin(ANALOG_INPUT_NONE), + _settle_time_ms(0) {} float ADCSource::read_average() { diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 96534cd892..4ed20c6ba4 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -279,17 +279,17 @@ void AP_MotorsMatrix::output_disarmed() // output_disarmed - sends commands to the motors void AP_MotorsMatrix::output_test() { - int8_t min_order, max_order; - int8_t i,j; + uint8_t min_order, max_order; + uint8_t i,j; // find min and max orders - min_order = test_order[0]; - max_order = test_order[0]; + min_order = _test_order[0]; + max_order = _test_order[0]; for(i=1; i max_order ) - max_order = test_order[i]; + if( _test_order[i] < min_order ) + min_order = _test_order[i]; + if( _test_order[i] > max_order ) + max_order = _test_order[i]; } // shut down all motors @@ -301,7 +301,7 @@ void AP_MotorsMatrix::output_test() // loop through all the possible orders spinning any motors that match that description for( i=min_order; i<=max_order; i++ ) { for( j=0; jwrite(_motor_to_channel_map[j], _rc_throttle->radio_min + 100); hal.scheduler->delay(300); @@ -316,7 +316,7 @@ void AP_MotorsMatrix::output_test() } // add_motor -void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t testing_order) +void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order) { // ensure valid motor number is provided if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { @@ -333,16 +333,12 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc _yaw_factor[motor_num] = yaw_fac; // set order that motor appears in test - if( testing_order == AP_MOTORS_MATRIX_ORDER_UNDEFINED ) { - test_order[motor_num] = motor_num; - }else{ - test_order[motor_num] = testing_order; - } + _test_order[motor_num] = testing_order; } } // add_motor using just position and prop direction -void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, int8_t testing_order) +void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order) { // call raw motor set-up method add_motor_raw( diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 82aa544547..fbc6b96149 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -11,9 +11,6 @@ #include // RC Channel Library #include "AP_Motors_Class.h" -#define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1 -#define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1 - #define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1 #define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1 @@ -49,7 +46,7 @@ public: virtual void output_min(); // add_motor using just position and yaw_factor (or prop direction) - void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED); + void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); // remove_motor void remove_motor(int8_t motor_num); @@ -62,21 +59,19 @@ public: remove_all_motors(); }; - // matrix - AP_Int8 test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence - protected: // output - sends commands to the motors virtual void output_armed(); virtual void output_disarmed(); // add_motor using raw roll, pitch, throttle and yaw factors - void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED); + void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order); int8_t _num_motors; // not a very useful variable as you really need to check the motor_enabled array to see which motors are enabled float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1) + uint8_t _test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence }; #endif // AP_MOTORSMATRIX