Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

CoaxCopter: motor_to_channel_map moved to progmem

This commit is contained in:
Randy Mackay 2014-02-13 11:52:44 +09:00 committed by Andrew Tridgell
parent 3fad8e3630
commit 1bee56877c
2 changed files with 47 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -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();

6
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -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