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

AP_Motors: spool state moved from multi to parent 

Also rename SPIN_WHEN_ARMED to GROUND_IDLE
This commit is contained in:
bnsgeyer 2018-12-28 17:01:38 +10:30 committed by Randy Mackay
parent bee90261ef
commit ac87b3e1e5
10 changed files with 36 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -239,7 +239,7 @@ void AP_Motors6DOF::output_to_motors()
}
}
break;
case SPIN_WHEN_ARMED:
case GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {

2
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -78,7 +78,7 @@ void AP_MotorsCoax::output_to_motors()
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:
case GROUND_IDLE:
// sends output to motors when armed but not flying
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);

2
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -89,7 +89,7 @@ void AP_MotorsMatrix::output_to_motors()
}
break;
}
case SPIN_WHEN_ARMED:
case GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {

11
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -191,7 +191,6 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// Constructor
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_spool_mode(SHUT_DOWN),
_lift_max(1.0f),
_throttle_limit(1.0f)
{
@ -457,7 +456,7 @@ void AP_MotorsMulticopter::output_logic()
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_spool_mode = SPIN_WHEN_ARMED;
_spool_mode = GROUND_IDLE;
break;
}
@ -470,8 +469,8 @@ void AP_MotorsMulticopter::output_logic()
_thrust_boost_ratio = 0.0f;
break;
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
case GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
// set limits flags
@ -496,7 +495,7 @@ void AP_MotorsMulticopter::output_logic()
_spin_up_ratio = 1.0f;
_spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
} else { // _spool_desired == GROUND_IDLE
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
@ -597,7 +596,7 @@ void AP_MotorsMulticopter::output_logic()
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) {
_spool_mode = SPIN_WHEN_ARMED;
_spool_mode = GROUND_IDLE;
}
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate));

10
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -53,15 +53,6 @@ public:
void update_throttle_hover(float dt);
virtual float get_throttle_hover() const override { return _throttle_hover; }
// spool up states
enum spool_up_down_mode {
SHUT_DOWN = 0, // all motors stop
SPIN_WHEN_ARMED = 1, // all motors at spin when armed
SPOOL_UP = 2, // increasing maximum throttle while stabilizing
THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure
SPOOL_DOWN = 4, // decreasing maximum throttle while stabilizing
};
// passes throttle directly to all motors for ESC calibration.
// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void set_throttle_passthrough_for_esc_calibration(float throttle_input);
@ -180,7 +171,6 @@ protected:
int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX)
// spool variables
spool_up_down_mode _spool_mode; // motor's current spool mode
float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
// battery voltage, current and air pressure compensation variables

2
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -81,7 +81,7 @@ void AP_MotorsSingle::output_to_motors()
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:
case GROUND_IDLE:
// sends output to motors when armed but not flying
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);

2
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -89,7 +89,7 @@ void AP_MotorsTailsitter::output_to_motors()
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:
case GROUND_IDLE:
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());

2
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -85,7 +85,7 @@ void AP_MotorsTri::output_to_motors()
rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPIN_WHEN_ARMED:
case GROUND_IDLE:
// sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());

8
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -35,6 +35,7 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_speed_hz(speed_hz),
_throttle_filter(),
_spool_desired(DESIRED_SHUT_DOWN),
_spool_mode(SHUT_DOWN),
_air_density_ratio(1.0f)
{
_instance = this;
@ -63,6 +64,13 @@ void AP_Motors::armed(bool arm)
}
};
void AP_Motors::set_desired_spool_state(enum spool_up_down_desired spool)
{
if (_flags.armed || (spool == DESIRED_SHUT_DOWN)) {
_spool_desired = spool;
}
};
// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input)
{

21
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -101,17 +101,29 @@ public:
bool get_thrust_boost() const { return _thrust_boost; }
virtual uint8_t get_lost_motor() const { return 0; }
// spool up states
// desired spool states
enum spool_up_down_desired {
DESIRED_SHUT_DOWN = 0, // all motors stop
DESIRED_SPIN_WHEN_ARMED = 1, // all motors at spin when armed
DESIRED_GROUND_IDLE = 1, // all motors at ground idle
DESIRED_THROTTLE_UNLIMITED = 2, // motors are no longer constrained by start up procedure
};
virtual void set_desired_spool_state(enum spool_up_down_desired spool) { _spool_desired = spool; };
void set_desired_spool_state(enum spool_up_down_desired spool);
enum spool_up_down_desired get_desired_spool_state(void) const { return _spool_desired; }
// spool states
enum spool_up_down_mode {
SHUT_DOWN = 0, // all motors stop
GROUND_IDLE = 1, // all motors at ground idle
SPOOL_UP = 2, // increasing maximum throttle while stabilizing
THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure
SPOOL_DOWN = 4, // decreasing maximum throttle while stabilizing
};
// get_spool_mode - get current spool mode
enum spool_up_down_mode get_spool_mode(void) const { return _spool_mode; }
// set_density_ratio - sets air density as a proportion of sea level density
void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
@ -207,6 +219,7 @@ protected:
float _throttle_avg_max; // last throttle input from set_throttle_avg_max
LowPassFilterFloat _throttle_filter; // throttle input filter
spool_up_down_desired _spool_desired; // desired spool state
spool_up_down_mode _spool_mode; // current spool mode
// air pressure compensation variables
float _air_density_ratio; // air density / sea level density - decreases in altitude