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

AP_Motors: Tailsitter output using SRV_Channels instead of rc_write

master
Randy Mackay 6 years ago
parent
11c29b6af9
commit
0d8e02d113
1 changed files with 57 additions and 51 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. 108
      libraries/AP_Motors/AP_MotorsTailsitter.cpp

108
libraries/AP_Motors/AP_MotorsTailsitter.cpp
Unescape View File

@ -26,20 +26,32 @@ @@ -26,20 +26,32 @@
extern const AP_HAL::HAL& hal;
#define SERVO_OUTPUT_RANGE 4500
#define THROTTLE_RANGE 100
// init
void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// make sure 4 output channels are mapped
add_motor_num(AP_MOTORS_THROTTLE_LEFT);
add_motor_num(AP_MOTORS_THROTTLE_RIGHT);
add_motor_num(AP_MOTORS_TILT_LEFT);
add_motor_num(AP_MOTORS_TILT_RIGHT);
// setup default motor and servo mappings
uint8_t chan;
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_THROTTLE_LEFT] = true;
motor_enabled[AP_MOTORS_THROTTLE_RIGHT] = true;
// right throttle defaults to servo output 1
SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleRight, CH_1);
if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) {
motor_enabled[chan] = true;
}
// left throttle defaults to servo output 2
SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleLeft, CH_2);
if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) {
motor_enabled[chan] = true;
}
// right servo defaults to servo output 3
SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorRight, CH_3);
SRV_Channels::set_angle(SRV_Channel::k_tiltMotorRight, SERVO_OUTPUT_RANGE);
// left servo defaults to servo output 4
SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorLeft, CH_4);
SRV_Channels::set_angle(SRV_Channel::k_tiltMotorLeft, SERVO_OUTPUT_RANGE);
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER);
@ -50,8 +62,7 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f @@ -50,8 +62,7 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f
AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) :
AP_MotorsMulticopter(loop_rate, speed_hz)
{
SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz);
SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
set_update_rate(speed_hz);
}
@ -61,10 +72,8 @@ void AP_MotorsTailsitter::set_update_rate(uint16_t speed_hz) @@ -61,10 +72,8 @@ void AP_MotorsTailsitter::set_update_rate(uint16_t speed_hz)
// record requested speed
_speed_hz = speed_hz;
uint32_t mask =
1U << AP_MOTORS_THROTTLE_LEFT |
1U << AP_MOTORS_THROTTLE_RIGHT;
rc_set_freq(mask, _speed_hz);
SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz);
SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
}
void AP_MotorsTailsitter::output_to_motors()
@ -72,59 +81,56 @@ void AP_MotorsTailsitter::output_to_motors() @@ -72,59 +81,56 @@ void AP_MotorsTailsitter::output_to_motors()
if (!_flags.initialised_ok) {
return;
}
float throttle = 0.0f;
float throttle_pwm = 0.0f;
switch (_spool_mode) {
case SHUT_DOWN:
throttle = get_pwm_output_min();
rc_write(AP_MOTORS_THROTTLE_LEFT, get_pwm_output_min());
rc_write(AP_MOTORS_THROTTLE_RIGHT, get_pwm_output_min());
throttle_pwm = get_pwm_output_min();
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, get_pwm_output_min());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min());
break;
case SPIN_WHEN_ARMED:
throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
rc_write(AP_MOTORS_THROTTLE_LEFT, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_spin_up_to_pwm());
throttle_pwm = calc_spin_up_to_pwm();
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_spin_up_to_pwm());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_spin_up_to_pwm());
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
throttle = calc_thrust_to_pwm(_throttle);
rc_write(AP_MOTORS_THROTTLE_LEFT, calc_thrust_to_pwm(_thrust_left));
rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_thrust_to_pwm(_thrust_right));
throttle_pwm = calc_thrust_to_pwm(_throttle);
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_thrust_to_pwm(_thrust_left));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_thrust_to_pwm(_thrust_right));
break;
}
// Always output to tilts
rc_write_angle(AP_MOTORS_TILT_LEFT, _tilt_left*SERVO_OUTPUT_RANGE);
rc_write_angle(AP_MOTORS_TILT_RIGHT, _tilt_right*SERVO_OUTPUT_RANGE);
// Always output to tilt servos
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, _tilt_left*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, _tilt_right*SERVO_OUTPUT_RANGE);
// plane outputs for Qmodes are setup here, and written to the HAL by the plane servos loop
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, -_yaw_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _pitch_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _roll_in*SERVO_OUTPUT_RANGE);
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle);
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
#endif
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle_pwm);
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsTailsitter::get_motor_mask()
{
uint32_t motor_mask =
1U << AP_MOTORS_THROTTLE_LEFT |
1U << AP_MOTORS_THROTTLE_RIGHT |
1U << AP_MOTORS_TILT_LEFT |
1U << AP_MOTORS_TILT_RIGHT;
uint16_t mask = rc_map_mask(motor_mask);
uint32_t motor_mask = 0;
uint8_t chan;
if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) {
motor_mask |= 1U << chan;
}
if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) {
motor_mask |= 1U << chan;
}
// add parent's mask
mask |= AP_MotorsMulticopter::get_motor_mask();
motor_mask |= AP_MotorsMulticopter::get_motor_mask();
return mask;
return motor_mask;
}
// calculate outputs to the motors
@ -189,20 +195,20 @@ void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm) @@ -189,20 +195,20 @@ void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm)
// output to motors and servos
switch (motor_seq) {
case 1:
// throttle left
rc_write(AP_MOTORS_THROTTLE_LEFT, pwm);
// right throttle
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm);
break;
case 2:
// throttle right
rc_write(AP_MOTORS_THROTTLE_RIGHT, pwm);
// right tilt servo
SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorRight, pwm);
break;
case 3:
// tilt left
rc_write(AP_MOTORS_TILT_LEFT, pwm);
// left throttle
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm);
break;
case 4:
// tilt right
rc_write(AP_MOTORS_TILT_RIGHT, pwm);
// left tilt servo
SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorLeft, pwm);
break;
default:
// do nothing

Write Preview
Loading…
Cancel
Save