Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsCoax: support 4 servo outputs

Browse Source
This commit is contained in:
Leonard Hall 2016-01-20 12:54:01 +09:00 committed by Randy Mackay
parent 7d6c6b5556
commit 7df9b2eb8c
2 changed files with 76 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -64,18 +64,25 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = {
// init
void AP_MotorsCoax::Init()
{
// set update rate for the 2 motors (but not the servo on channel 1&2)
// set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate(_speed_hz);
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_3] = true;
motor_enabled[AP_MOTORS_MOT_4] = true;
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true;
// set ranges for fin servos
// we set four servos to angle
_servo1.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo2.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo3.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo4.set_type(RC_CHANNEL_TYPE_ANGLE);
_servo1.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
_servo2.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
_servo3.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
_servo4.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
// disable CH7 from being used as an aux output (i.e. for camera gimbal, etc)
RC_Channel_aux::disable_aux_channel(CH_7);
}
// set update rate to motors - a value in hertz
@ -84,17 +91,17 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
// record requested speed
_speed_hz = speed_hz;
// set update rate for the two motors
uint32_t mask2 =
// set update rate for the 4 servos and 2 motors
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4 ;
rc_set_freq(mask2, _speed_hz);
// set update rate for the two servos
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 ;
rc_set_freq(mask, _servo_speed);
uint32_t mask2 =
1U << AP_MOTORS_MOT_5 |
1U << AP_MOTORS_MOT_6 ;
rc_set_freq(mask2, _speed_hz);
}
// enable - starts allowing signals to be sent to motors
@ -105,6 +112,8 @@ void AP_MotorsCoax::enable()
rc_enable_ch(AP_MOTORS_MOT_2);
rc_enable_ch(AP_MOTORS_MOT_3);
rc_enable_ch(AP_MOTORS_MOT_4);
rc_enable_ch(AP_MOTORS_MOT_5);
rc_enable_ch(AP_MOTORS_MOT_6);
}
// output_min - sends minimum values out to the motor and trim values to the servos
@ -114,11 +123,27 @@ void AP_MotorsCoax::output_min()
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, _servo1.radio_trim);
rc_write(AP_MOTORS_MOT_2, _servo2.radio_trim);
rc_write(AP_MOTORS_MOT_3, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_4, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_3, _servo3.radio_trim);
rc_write(AP_MOTORS_MOT_4, _servo4.radio_trim);
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
hal.rcout->push();
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsCoax::get_motor_mask()
{
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4 |
1U << AP_MOTORS_MOT_5 |
1U << AP_MOTORS_MOT_6;
return rc_map_mask(mask);
}
// sends commands to the motors
void AP_MotorsCoax::output_armed_stabilizing()
{
@ -199,13 +224,21 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm)
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// motor 1
// flap servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// motor 2
// flap servo 4
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// motor 1
rc_write(AP_MOTORS_MOT_5, pwm);
break;
case 6:
// motor 2
rc_write(AP_MOTORS_MOT_6, pwm);
break;
default:
// do nothing
break;

30
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -23,10 +23,12 @@ class AP_MotorsCoax : public AP_MotorsMulticopter {
public:
/// Constructor
AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMulticopter(loop_rate, speed_hz),
_servo1(servo1),
_servo2(servo2)
_servo2(servo2),
_servo3(servo3),
_servo4(servo4)
{
AP_Param::setup_object_defaults(this, var_info);
};
@ -50,8 +52,7 @@ public:
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
// for coax copter, output channels 1 to 4 are used
virtual uint16_t get_motor_mask() { return 0x000F; }
virtual uint16_t get_motor_mask();
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -65,6 +66,27 @@ protected:
AP_Int8 _pitch_reverse; // Reverse pitch output
AP_Int8 _yaw_reverse; // Reverse yaw output
AP_Int16 _servo_speed; // servo speed
// Allow the use of a 4 servo output to make it easy to test coax and single using same airframe
RC_Channel& _servo1;
RC_Channel& _servo2;
RC_Channel& _servo3;
RC_Channel& _servo4;
AP_Int8 _servo_1_reverse; // Roll servo signal reversing
AP_Int16 _servo_1_trim; // Trim or center position of roll servo
AP_Int16 _servo_1_min; // Minimum angle limit of roll servo
AP_Int16 _servo_1_max; // Maximum angle limit of roll servo
AP_Int8 _servo_2_reverse; // Pitch servo signal reversing
AP_Int16 _servo_2_trim; // Trim or center position of pitch servo
AP_Int16 _servo_2_min; // Minimum angle limit of pitch servo
AP_Int16 _servo_2_max; // Maximum angle limit of pitch servo
AP_Int8 _servo_3_reverse; // Pitch servo signal reversing
AP_Int16 _servo_3_trim; // Trim or center position of pitch servo
AP_Int16 _servo_3_min; // Minimum angle limit of pitch servo
AP_Int16 _servo_3_max; // Maximum angle limit of pitch servo
AP_Int8 _servo_4_reverse; // Pitch servo signal reversing
AP_Int16 _servo_4_trim; // Trim or center position of pitch servo
AP_Int16 _servo_4_min; // Minimum angle limit of pitch servo
AP_Int16 _servo_4_max; // Maximum angle limit of pitch servo
};