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

AP_MotorsSingle: move servo objects into Single class

This commit is contained in:
Leonard Hall 2016-01-21 10:51:12 +09:00 committed by Randy Mackay
parent 21d304b86d
commit 6264159f4d
2 changed files with 43 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -59,6 +59,19 @@ const AP_Param::GroupInfo AP_MotorsSingle::var_info[] = {
// @Values: 50, 125, 250
AP_GROUPINFO("SV_SPEED", 43, AP_MotorsSingle, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS),
// @Group: SV1_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo1, "SV1_", 44, AP_MotorsSingle, RC_Channel),
// @Group: SV2_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo2, "SV2_", 45, AP_MotorsSingle, RC_Channel),
// @Group: SV3_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo3, "SV3_", 46, AP_MotorsSingle, RC_Channel),
// @Group: SV4_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo4, "SV4_", 47, AP_MotorsSingle, RC_Channel),
AP_GROUPEND
};
// init
@ -136,10 +149,10 @@ void AP_MotorsSingle::output_to_motors()
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(constrain_float(_roll_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f) *_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(constrain_float(_pitch_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f) *_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(constrain_float(-_roll_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f) *_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(constrain_float(-_pitch_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f) *_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max));
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(constrain_float(_roll_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f), _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(constrain_float(_pitch_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f), _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(constrain_float(-_roll_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f), _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(constrain_float(-_pitch_radio_passthrough+_yaw_radio_passthrough, -1.0f, 1.0f), _servo4));
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
hal.rcout->push();
@ -147,10 +160,10 @@ void AP_MotorsSingle::output_to_motors()
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[0]*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[1]*_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[2]*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[3]*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max));
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
hal.rcout->push();
@ -160,10 +173,10 @@ void AP_MotorsSingle::output_to_motors()
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_actuator_out[0]*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_actuator_out[1]*_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_actuator_out[2]*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_actuator_out[3]*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max));
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
@ -351,14 +364,18 @@ void AP_MotorsSingle::output_test(uint8_t motor_seq, int16_t pwm)
}
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t AP_MotorsSingle::calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max)
int16_t AP_MotorsSingle::calc_pivot_radio_output(float yaw_input, const RC_Channel& servo)
{
int16_t ret;
if (yaw_input >= 0){
ret = ((yaw_input * (servo_max - servo_trim)) + servo_trim);
if (servo.get_reverse()) {
yaw_input = -yaw_input;
}
if (yaw_input >= 0.0f){
ret = ((yaw_input * (servo.radio_max - servo.radio_trim)) + servo.radio_trim);
} else {
ret = ((yaw_input * (servo_trim - servo_min)) + servo_trim);
ret = ((yaw_input * (servo.radio_trim - servo.radio_min)) + servo.radio_trim);
}
return ret;

38
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -25,12 +25,9 @@ class AP_MotorsSingle : public AP_MotorsMulticopter {
public:
/// Constructor
AP_MotorsSingle(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_MotorsSingle(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMulticopter(loop_rate, speed_hz),
_servo1(servo1),
_servo2(servo2),
_servo3(servo3),
_servo4(servo4)
_servo1(CH_1), _servo2(CH_2), _servo3(CH_3), _servo4(CH_4)
{
AP_Param::setup_object_defaults(this, var_info);
};
@ -66,36 +63,21 @@ protected:
// output - sends commands to the motors
void output_armed_stabilizing();
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max); // calculate radio output for yaw servo, typically in range of 1100-1900
// calc_yaw_radio_output - calculate final pwm output for yaw channel
int16_t calc_pivot_radio_output(float yaw_input, const RC_Channel& servo);
// We shouldn't need roll, pitch, and yaw reversing with servo reversing.
AP_Int8 _roll_reverse; // Reverse roll output
AP_Int8 _pitch_reverse; // Reverse pitch output
AP_Int8 _yaw_reverse; // Reverse yaw output
AP_Int16 _servo_speed; // servo speed
RC_Channel& _servo1;
RC_Channel& _servo2;
RC_Channel& _servo3;
RC_Channel& _servo4;
int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
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
RC_Channel _servo1;
RC_Channel _servo2;
RC_Channel _servo3;
RC_Channel _servo4;
int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
float _thrust_out;
};