zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: use scaled actuators for single and coax frames 

this allows for multiple servos with the same motor function
mission-4.1.18
Andrew Tridgell 7 years ago
parent
a17979383d
commit
4e414c02f5
6 changed files with 45 additions and 70 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 48
      libraries/AP_Motors/AP_MotorsCoax.cpp
  2. 4
      libraries/AP_Motors/AP_MotorsCoax.h
  3. 49
      libraries/AP_Motors/AP_MotorsSingle.cpp
  4. 4
      libraries/AP_Motors/AP_MotorsSingle.h
  5. 9
      libraries/AP_Motors/AP_Motors_Class.cpp
  6. 1
      libraries/AP_Motors/AP_Motors_Class.h

48
libraries/AP_Motors/AP_MotorsCoax.cpp
Unescape View File

@ -30,29 +30,19 @@ extern const AP_HAL::HAL& hal;
// init // init
void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type) void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type)
{ {
_servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); // make sure 6 output channels are mapped
_servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); for (uint8_t i=0; i<6; i++) {
_servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); add_motor_num(CH_1+i);
_servo4 = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4);
if (!_servo1 || !_servo2 || !_servo3 || !_servo4) {
gcs().send_text(MAV_SEVERITY_ERROR, "MotorsCoax: unable to setup output channels");
// don't set initialised_ok
return;
} }
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types // 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_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true; motor_enabled[AP_MOTORS_MOT_6] = true;
// we set four servos to angle // setup actuator scaling
_servo1->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_servo2->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_COAX_SERVO_INPUT_RANGE);
_servo3->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); }
_servo4->set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
// allow mapping of motors 5 and 6
add_motor_num(CH_5);
add_motor_num(CH_6);
// record successful initialisation if what we setup was the desired frame_class // record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX); _flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX);
@ -81,19 +71,18 @@ void AP_MotorsCoax::output_to_motors()
switch (_spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough, _servo1)); rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2)); rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough, _servo3)); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough, _servo4)); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_5, get_pwm_output_min()); rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_output_min()); rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
break; break;
case SPIN_WHEN_ARMED: case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1)); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2)); rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3)); }
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
break; break;
@ -101,10 +90,9 @@ void AP_MotorsCoax::output_to_motors()
case THROTTLE_UNLIMITED: case THROTTLE_UNLIMITED:
case SPOOL_DOWN: case SPOOL_DOWN:
// set motor output based on thrust requests // set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1)); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2)); rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3)); }
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_yt_ccw)); 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)); rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw));
break; break;

4
libraries/AP_Motors/AP_MotorsCoax.h
Unescape View File

@ -56,8 +56,4 @@ protected:
float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
float _thrust_yt_ccw; float _thrust_yt_ccw;
float _thrust_yt_cw; float _thrust_yt_cw;
SRV_Channel *_servo1;
SRV_Channel *_servo2;
SRV_Channel *_servo3;
SRV_Channel *_servo4;
}; };

49
libraries/AP_Motors/AP_MotorsSingle.cpp
Unescape View File

@ -30,32 +30,19 @@ extern const AP_HAL::HAL& hal;
// init // init
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type) void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
{ {
// set update rate for the 3 motors (but not the servo on channel 7) // make sure 6 output channels are mapped
set_update_rate(_speed_hz); for (uint8_t i=0; i<6; i++) {
add_motor_num(CH_1+i);
}
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types // 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_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true; motor_enabled[AP_MOTORS_MOT_6] = true;
_servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); // setup actuator scaling
_servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo4 = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4);
if (!_servo1 || !_servo2 || !_servo3 || !_servo4) {
gcs().send_text(MAV_SEVERITY_ERROR, "MotorsSingle: unable to setup output channels");
// don't set initialised_ok
return;
} }
// we set four servos to angle
_servo1->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo2->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo3->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo4->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
// allow mapping of motors 5 and 6
add_motor_num(CH_5);
add_motor_num(CH_6);
// record successful initialisation if what we setup was the desired frame_class // record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE); _flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
@ -87,19 +74,18 @@ void AP_MotorsSingle::output_to_motors()
switch (_spool_mode) { switch (_spool_mode) {
case SHUT_DOWN: case SHUT_DOWN:
// sends minimum values out to the motors // sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough - _yaw_radio_passthrough, _servo1)); rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough - _yaw_radio_passthrough, _servo2)); rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough - _yaw_radio_passthrough, _servo3)); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough - _yaw_radio_passthrough, _servo4)); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_5, get_pwm_output_min()); rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_output_min()); rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
break; break;
case SPIN_WHEN_ARMED: case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1)); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2)); rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3)); }
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
break; break;
@ -107,10 +93,9 @@ void AP_MotorsSingle::output_to_motors()
case THROTTLE_UNLIMITED: case THROTTLE_UNLIMITED:
case SPOOL_DOWN: case SPOOL_DOWN:
// set motor output based on thrust requests // set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1)); for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2)); rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3)); }
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out)); rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out)); rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
break; break;

4
libraries/AP_Motors/AP_MotorsSingle.h
Unescape View File

@ -56,8 +56,4 @@ protected:
int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 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 _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
float _thrust_out; float _thrust_out;
SRV_Channel *_servo1;
SRV_Channel *_servo2;
SRV_Channel *_servo3;
SRV_Channel *_servo4;
}; };

9
libraries/AP_Motors/AP_Motors_Class.cpp
Unescape View File

@ -90,6 +90,15 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
SRV_Channels::set_output_pwm(function, pwm); SRV_Channels::set_output_pwm(function, pwm);
} }
/*
write to an output channel for an angle actuator
*/
void AP_Motors::rc_write_angle(uint8_t chan, int16_t angle_cd)
{
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
SRV_Channels::set_output_scaled(function, angle_cd);
}
/* /*
set frequency of a set of channels set frequency of a set of channels
*/ */

1
libraries/AP_Motors/AP_Motors_Class.h
Unescape View File

@ -165,6 +165,7 @@ protected:
// output functions that should be overloaded by child classes // output functions that should be overloaded by child classes
virtual void output_armed_stabilizing()=0; virtual void output_armed_stabilizing()=0;
virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_write(uint8_t chan, uint16_t pwm);
virtual void rc_write_angle(uint8_t chan, int16_t angle_cd);
virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);
virtual uint32_t rc_map_mask(uint32_t mask) const; virtual uint32_t rc_map_mask(uint32_t mask) const;

Write Preview
Loading…
Cancel
Save