zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_MotorsCoax: move servo objects into Coax class

master
Leonard Hall 9 years ago committed by Randy Mackay
parent
110d41ee24
commit
260006dcb3
2 changed files with 42 additions and 44 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      libraries/AP_Motors/AP_MotorsCoax.cpp
  2. 37
      libraries/AP_Motors/AP_MotorsCoax.h

49
libraries/AP_Motors/AP_MotorsCoax.cpp
Unescape View File

@ -59,6 +59,19 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = { @@ -59,6 +59,19 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = {
// @Units: Hz
AP_GROUPINFO("SV_SPEED", 43, AP_MotorsCoax, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS),
// @Group: SV1_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo1, "SV1_", 44, AP_MotorsCoax, RC_Channel),
// @Group: SV2_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo2, "SV2_", 45, AP_MotorsCoax, RC_Channel),
// @Group: SV3_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo3, "SV3_", 46, AP_MotorsCoax, RC_Channel),
// @Group: SV4_
// @Path: ../RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(_servo4, "SV4_", 47, AP_MotorsCoax, RC_Channel),
AP_GROUPEND
};
// init
@ -136,10 +149,10 @@ void AP_MotorsCoax::output_to_motors() @@ -136,10 +149,10 @@ void AP_MotorsCoax::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(_roll_radio_passthrough*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_pitch_radio_passthrough *_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_roll_radio_passthrough*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_pitch_radio_passthrough*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max));
rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_roll_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_pitch_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_roll_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_pitch_radio_passthrough, _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_MotorsCoax::output_to_motors() @@ -147,10 +160,10 @@ void AP_MotorsCoax::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_MotorsCoax::output_to_motors() @@ -160,10 +173,10 @@ void AP_MotorsCoax::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_yt_ccw));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw));
hal.rcout->push();
@ -338,14 +351,18 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm) @@ -338,14 +351,18 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm)
}
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t AP_MotorsCoax::calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max)
int16_t AP_MotorsCoax::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;

37
libraries/AP_Motors/AP_MotorsCoax.h
Unescape View File

@ -25,12 +25,9 @@ class AP_MotorsCoax : public AP_MotorsMulticopter { @@ -25,12 +25,9 @@ class AP_MotorsCoax : public AP_MotorsMulticopter {
public:
/// Constructor
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_MotorsCoax(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,20 @@ protected: @@ -66,36 +63,20 @@ 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
// 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
RC_Channel _servo1;
RC_Channel _servo2;
RC_Channel _servo3;
RC_Channel _servo4;
float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
float _thrust_yt_ccw;

Write Preview
Loading…
Cancel
Save