From 1bee56877cd6dab3f59c22cf6fb8f333ec00249d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 13 Feb 2014 11:52:44 +0900 Subject: [PATCH] CoaxCopter: motor_to_channel_map moved to progmem --- libraries/AP_Motors/AP_MotorsCoax.cpp | 93 +++++++++++++-------------- libraries/AP_Motors/AP_MotorsCoax.h | 6 +- 2 files changed, 47 insertions(+), 52 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index d09076add1..249e79f18e 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -100,10 +100,10 @@ void AP_MotorsCoax::Init() motor_enabled[AP_MOTORS_MOT_4] = true; // set ranges for fin servos - _servo1->set_type(RC_CHANNEL_TYPE_ANGLE); - _servo2->set_type(RC_CHANNEL_TYPE_ANGLE); - _servo1->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); - _servo2->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo1.set_type(RC_CHANNEL_TYPE_ANGLE); + _servo2.set_type(RC_CHANNEL_TYPE_ANGLE); + _servo1.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo2.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); } // set update rate to motors - a value in hertz @@ -114,14 +114,14 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) // set update rate for the two motors uint32_t mask2 = - 1U << _motor_to_channel_map[AP_MOTORS_MOT_3] | - 1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ; + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) | + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]) ; hal.rcout->set_freq(mask2, _speed_hz); // set update rate for the two servos uint32_t mask = - 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] | - 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ; + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) | + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) ; hal.rcout->set_freq(mask, _servo_speed); } @@ -129,42 +129,37 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) void AP_MotorsCoax::enable() { // enable output channels - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]); - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]); - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); } // output_min - sends minimum values out to the motor and trim values to the servos void AP_MotorsCoax::output_min() { - // fill the motor_out[] array for HIL use - motor_out[AP_MOTORS_MOT_1] = _servo1->radio_trim; - motor_out[AP_MOTORS_MOT_2] = _servo2->radio_trim; - motor_out[AP_MOTORS_MOT_3] = _rc_throttle->radio_min; - motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min; - // send minimum value to each motor - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _rc_throttle->radio_min); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_trim); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); } // output_armed - sends commands to the motors void AP_MotorsCoax::output_armed() { - int16_t out_min = _rc_throttle->radio_min + _min_throttle; + int16_t out_min = _rc_throttle.radio_min + _min_throttle; + int16_t motor_out[4]; // Throttle is 0 to 1000 only - _rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle); + _rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle); // capture desired throttle and yaw from receiver - _rc_throttle->calc_pwm(); - _rc_yaw->calc_pwm(); + _rc_throttle.calc_pwm(); + _rc_yaw.calc_pwm(); // if we are not sending a throttle output, we cut the motors - if(_rc_throttle->servo_out == 0) { + if(_rc_throttle.servo_out == 0) { // range check spin_when_armed if (_spin_when_armed < 0) { _spin_when_armed = 0; @@ -172,19 +167,19 @@ void AP_MotorsCoax::output_armed() if (_spin_when_armed > _min_throttle) { _spin_when_armed = _min_throttle; } - motor_out[AP_MOTORS_MOT_3] = _rc_throttle->radio_min + _spin_when_armed; - motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed; + motor_out[AP_MOTORS_MOT_3] = _rc_throttle.radio_min + _spin_when_armed; + motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed; }else{ // motors - motor_out[AP_MOTORS_MOT_3] = _rev_yaw*_rc_yaw->pwm_out + _rc_throttle->radio_out; - motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*_rc_yaw->pwm_out +_rc_throttle->radio_out; + motor_out[AP_MOTORS_MOT_3] = _rev_yaw*_rc_yaw.pwm_out + _rc_throttle.radio_out; + motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*_rc_yaw.pwm_out +_rc_throttle.radio_out; // front - _servo1->servo_out = _rev_roll*_rc_roll->servo_out; + _servo1.servo_out = _rev_roll*_rc_roll.servo_out; // right - _servo2->servo_out = _rev_pitch*_rc_pitch->servo_out; - _servo1->calc_pwm(); - _servo2->calc_pwm(); + _servo2.servo_out = _rev_pitch*_rc_pitch.servo_out; + _servo1.calc_pwm(); + _servo2.calc_pwm(); // adjust for throttle curve if( _throttle_curve_enabled ) { @@ -198,10 +193,10 @@ void AP_MotorsCoax::output_armed() } // send output to each motor - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_out); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_out); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], motor_out[AP_MOTORS_MOT_3]); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), motor_out[AP_MOTORS_MOT_3]); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]); } // output_disarmed - sends commands to the motors @@ -219,32 +214,32 @@ void AP_MotorsCoax::output_test() // spin motor 1 hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _rc_throttle->radio_min + _min_throttle); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min + _min_throttle); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _rc_throttle->radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min); hal.scheduler->delay(2000); // spin motor 2 hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + _min_throttle); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min + _min_throttle); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); hal.scheduler->delay(2000); // flap servo 1 - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_min); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_max); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_max); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo1->radio_trim); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_trim); hal.scheduler->delay(2000); // flap servo 2 - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_min); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_max); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_max); hal.scheduler->delay(1000); - hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim); // Send minimum values to all motors output_min(); diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index ebd27240f4..ef73cc3441 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -25,7 +25,7 @@ class AP_MotorsCoax : public AP_Motors { public: /// Constructor - AP_MotorsCoax( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, RC_Channel* servo1, RC_Channel* servo2, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsCoax( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& servo1, RC_Channel& servo2, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz), _servo1(servo1), _servo2(servo2) @@ -60,8 +60,8 @@ protected: AP_Int8 _rev_pitch; // REV pitch feedback AP_Int8 _rev_yaw; // REV yaw feedback AP_Int16 _servo_speed; // servo speed - RC_Channel* _servo1; - RC_Channel* _servo2; + RC_Channel& _servo1; + RC_Channel& _servo2; }; #endif // AP_MOTORSCOAX