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

AP_Motors: use scaled actuators for single and coax frames 

this allows for multiple servos with the same motor function
This commit is contained in:
Andrew Tridgell 2018-05-21 11:37:38 +10:00
parent a17979383d
commit 4e414c02f5
6 changed files with 45 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -30,29 +30,19 @@ extern const AP_HAL::HAL& hal;
// init
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);
_servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2);
_servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3);
_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;
// make sure 6 output channels are mapped
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
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true;
// we set four servos to 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);
// allow mapping of motors 5 and 6
add_motor_num(CH_5);
add_motor_num(CH_6);
// setup actuator scaling
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_COAX_SERVO_INPUT_RANGE);
}
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX);
@ -81,19 +71,18 @@ void AP_MotorsCoax::output_to_motors()
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough, _servo4));
rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
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_6, get_pwm_output_min());
break;
case SPIN_WHEN_ARMED:
// 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));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
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));
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
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_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
break;
@ -101,10 +90,9 @@ void AP_MotorsCoax::output_to_motors()
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
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));
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
}
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));
break;

4
libraries/AP_Motors/AP_MotorsCoax.h
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 _thrust_yt_ccw;
float _thrust_yt_cw;
SRV_Channel *_servo1;
SRV_Channel *_servo2;
SRV_Channel *_servo3;
SRV_Channel *_servo4;
};

49
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -30,32 +30,19 @@ extern const AP_HAL::HAL& hal;
// init
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)
set_update_rate(_speed_hz);
// make sure 6 output channels are mapped
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
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true;
_servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1);
_servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2);
_servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3);
_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;
// setup actuator scaling
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
// 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
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
@ -87,19 +74,18 @@ void AP_MotorsSingle::output_to_motors()
switch (_spool_mode) {
case SHUT_DOWN:
// 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(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough - _yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough - _yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough - _yaw_radio_passthrough, _servo4));
rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
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_6, get_pwm_output_min());
break;
case SPIN_WHEN_ARMED:
// 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));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
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));
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
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_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
break;
@ -107,10 +93,9 @@ void AP_MotorsSingle::output_to_motors()
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
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));
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
break;

4
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -56,8 +56,4 @@ protected:
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;
SRV_Channel *_servo1;
SRV_Channel *_servo2;
SRV_Channel *_servo3;
SRV_Channel *_servo4;
};

9
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -90,6 +90,15 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t 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
*/

1
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -165,6 +165,7 @@ protected:
// output functions that should be overloaded by child classes
virtual void output_armed_stabilizing()=0;
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 uint32_t rc_map_mask(uint32_t mask) const;