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

Coax: remove unused servo3, servo4 

Removed commented out code
This commit is contained in:
Randy Mackay 2014-02-06 23:39:30 +09:00
parent f48e106271
commit f60d94c4ab
2 changed files with 59 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

208
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -92,14 +92,18 @@ void AP_MotorsCoax::Init()
// call parent Init function to set-up throttle curve // call parent Init function to set-up throttle curve
AP_Motors::Init(); AP_Motors::Init();
// set update rate for the 3 motors (but not the servo on channel 7) // set update rate for the 2 motors (but not the 2 flaps (i.e. servos) on channels 3 and 4)
set_update_rate(_speed_hz); set_update_rate(_speed_hz);
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types // set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_1] = true; motor_enabled[AP_MOTORS_MOT_1] = true;
motor_enabled[AP_MOTORS_MOT_2] = true; motor_enabled[AP_MOTORS_MOT_2] = true;
motor_enabled[AP_MOTORS_MOT_3] = true;
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);
} }
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
@ -108,37 +112,24 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
// record requested speed // record requested speed
_speed_hz = speed_hz; _speed_hz = speed_hz;
// set update rate for the 3 motors (but not the servo on channel 7) // set update rate for the two motors
// uint32_t mask = uint32_t mask2 =
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] | 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] | 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ;
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_3] | hal.rcout->set_freq(mask2, _speed_hz);
// 1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
// hal.rcout->set_freq(mask, _servo_speed);
//uint32_t mask2 = 1U << _motor_to_channel_map[AP_MOTORS_MOT_7];
//hal.rcout->set_freq(mask2, _speed_hz);
uint32_t mask = // set update rate for the two servos
1U << _motor_to_channel_map[AP_MOTORS_MOT_3] | uint32_t mask =
1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ; 1U << _motor_to_channel_map[AP_MOTORS_MOT_3] |
hal.rcout->set_freq(mask, _servo_speed); 1U << _motor_to_channel_map[AP_MOTORS_MOT_4] ;
uint32_t mask2 = hal.rcout->set_freq(mask, _servo_speed);
1U << _motor_to_channel_map[AP_MOTORS_MOT_1] |
1U << _motor_to_channel_map[AP_MOTORS_MOT_2] ;
hal.rcout->set_freq(mask2, _speed_hz);
} }
// enable - starts allowing signals to be sent to motors // enable - starts allowing signals to be sent to motors
void AP_MotorsCoax::enable() void AP_MotorsCoax::enable()
{ {
// enable output channels // 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_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(_motor_to_channel_map[AP_MOTORS_MOT_7]);
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_2]);
hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); 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(_motor_to_channel_map[AP_MOTORS_MOT_4]);
@ -148,32 +139,16 @@ void AP_MotorsCoax::enable()
void AP_MotorsCoax::output_min() void AP_MotorsCoax::output_min()
{ {
// fill the motor_out[] array for HIL use // fill the motor_out[] array for HIL use
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
// motor_out[AP_MOTORS_MOT_1] = _servo1->radio_trim;
// motor_out[AP_MOTORS_MOT_2] = _servo2->radio_trim;
// motor_out[AP_MOTORS_MOT_3] = _servo3->radio_trim;
// motor_out[AP_MOTORS_MOT_4] = _servo4->radio_trim;
// motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min; motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
motor_out[AP_MOTORS_MOT_3] = _servo3->radio_trim; motor_out[AP_MOTORS_MOT_3] = _servo1->radio_trim;
motor_out[AP_MOTORS_MOT_4] = _servo4->radio_trim; motor_out[AP_MOTORS_MOT_4] = _servo2->radio_trim;
// send minimum value to each motor // send minimum value to each motor
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_trim);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo2->radio_trim);
// 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], _servo3->radio_trim);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min);
} }
// output_armed - sends commands to the motors // output_armed - sends commands to the motors
@ -198,76 +173,37 @@ void AP_MotorsCoax::output_armed()
} }
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed; motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed; motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
//motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_min + _spin_when_armed;
}else{ }else{
//motors // motors
motor_out[AP_MOTORS_MOT_1] = _rev_yaw*_rc_yaw->servo_out + _rc_throttle->radio_out; motor_out[AP_MOTORS_MOT_1] = _rev_yaw*_rc_yaw->servo_out + _rc_throttle->radio_out;
motor_out[AP_MOTORS_MOT_2] = -_rev_yaw*_rc_yaw->servo_out +_rc_throttle->radio_out; motor_out[AP_MOTORS_MOT_2] = -_rev_yaw*_rc_yaw->servo_out +_rc_throttle->radio_out;
//Front // front
_servo3->servo_out = _rev_roll*_rc_roll->servo_out; _servo1->servo_out = _rev_roll*_rc_roll->servo_out;
//right // right
_servo4->servo_out = _rev_pitch*_rc_pitch->servo_out; _servo2->servo_out = _rev_pitch*_rc_pitch->servo_out;
_servo1->calc_pwm();
_servo2->calc_pwm();
motor_out[AP_MOTORS_MOT_3] = _servo1->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo2->radio_out;
////motor
//motor_out[AP_MOTORS_MOT_7] = _rc_throttle->radio_out;
// //front
// _servo1->servo_out = _rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out;
////right
// _servo2->servo_out = _rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
////rear
//_servo3->servo_out = -_rev_roll*_rc_roll->servo_out + _rev_yaw*_rc_yaw->servo_out; // - before _rev to reverse servo, and below
////left
//_servo4->servo_out = -_rev_pitch*_rc_pitch->servo_out + _rev_yaw*_rc_yaw->servo_out;
_servo3->calc_pwm();
_servo4->calc_pwm();
motor_out[AP_MOTORS_MOT_3] = _servo3->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo4->radio_out;
//_servo1->calc_pwm();
//_servo2->calc_pwm();
//_servo3->calc_pwm();
//_servo4->calc_pwm();
//motor_out[AP_MOTORS_MOT_1] = _servo1->radio_out;
//motor_out[AP_MOTORS_MOT_2] = _servo2->radio_out;
//motor_out[AP_MOTORS_MOT_3] = _servo3->radio_out;
//motor_out[AP_MOTORS_MOT_4] = _servo4->radio_out;
// adjust for throttle curve // adjust for throttle curve
if( _throttle_curve_enabled ) { if( _throttle_curve_enabled ) {
motor_out[AP_MOTORS_MOT_1] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_1]);
motor_out[AP_MOTORS_MOT_1] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_1]); motor_out[AP_MOTORS_MOT_2] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_2]);
motor_out[AP_MOTORS_MOT_2] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_2]);
//motor_out[AP_MOTORS_MOT_7] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_7]);
} }
// ensure motors don't drop below a minimum value and stop // ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min); motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min); motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min);
//motor_out[AP_MOTORS_MOT_7] = max(motor_out[AP_MOTORS_MOT_7], out_min);
} }
// send output to each motor // send output to each motor
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
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_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(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
// 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(_motor_to_channel_map[AP_MOTORS_MOT_7], motor_out[AP_MOTORS_MOT_7]);
} }
// output_disarmed - sends commands to the motors // output_disarmed - sends commands to the motors
@ -283,74 +219,34 @@ void AP_MotorsCoax::output_test()
// Send minimum values to all motors // Send minimum values to all motors
output_min(); output_min();
// spin motor 1 // spin motor 1
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + _min_throttle); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + _min_throttle);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
hal.scheduler->delay(2000); hal.scheduler->delay(2000);
// spin motor 2 // spin motor 2
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + _min_throttle); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + _min_throttle);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
hal.scheduler->delay(2000); hal.scheduler->delay(2000);
// flap servo 3 // flap servo 1
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_min);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_max); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_max);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo1->radio_trim);
hal.scheduler->delay(2000); hal.scheduler->delay(2000);
// flap servo 4 // flap servo 2
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_min); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo2->radio_min);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_max); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo2->radio_max);
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim); hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo2->radio_trim);
// // spin main motor
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _rc_throttle->radio_min + _min_throttle);
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_7], _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.scheduler->delay(1000);
// hal.rcout->write(_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.scheduler->delay(2000);
// // flap servo 2
// hal.rcout->write(_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.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo2->radio_trim);
// hal.scheduler->delay(2000);
// // flap servo 3
//hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_min);
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_max);
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo3->radio_trim);
// hal.scheduler->delay(2000);
// // flap servo 4
//hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_min);
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_max);
// hal.scheduler->delay(1000);
// hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo4->radio_trim);
// Send minimum values to all motors // Send minimum values to all motors
output_min(); output_min();

16
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -1,7 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_MotorsSingle.h /// @file AP_MotorsCoax.h
/// @brief Motor and Servo control class for Singlecopters /// @brief Motor and Servo control class for Co-axial helicopters with two motors and two flaps
#ifndef __AP_MOTORS_COAX_H__ #ifndef __AP_MOTORS_COAX_H__
#define __AP_MOTORS_COAX_H__ #define __AP_MOTORS_COAX_H__
@ -18,17 +18,17 @@
#define AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS 250 // update rate for digital servos #define AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS 250 // update rate for digital servos
#define AP_MOTORS_SINGLE_SPEED_ANALOG_SERVOS 125 // update rate for analog servos #define AP_MOTORS_SINGLE_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
#define AP_MOTORS_COAX_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max)
/// @class AP_MotorsSingle /// @class AP_MotorsSingle
class AP_MotorsCoax : public AP_Motors { class AP_MotorsCoax : public AP_Motors {
public: public:
/// Constructor /// 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, RC_Channel* servo3, RC_Channel* servo4, 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), AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
_servo1(servo1), _servo1(servo1),
_servo2(servo2), _servo2(servo2)
_servo3(servo3),
_servo4(servo4)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
}; };
@ -62,8 +62,6 @@ protected:
AP_Int16 _servo_speed; // servo speed AP_Int16 _servo_speed; // servo speed
RC_Channel* _servo1; RC_Channel* _servo1;
RC_Channel* _servo2; RC_Channel* _servo2;
RC_Channel* _servo3;
RC_Channel* _servo4;
}; };
#endif // AP_MOTORSSINGLE #endif // AP_MOTORSCOAX