Browse Source

AP_Motors: actuator_spin_up renamed to include _to_ground_idle

master
Randy Mackay 6 years ago
parent
commit
1fa335a77b
  1. 4
      libraries/AP_Motors/AP_MotorsCoax.cpp
  2. 2
      libraries/AP_Motors/AP_MotorsMatrix.cpp
  3. 4
      libraries/AP_Motors/AP_MotorsMulticopter.cpp
  4. 4
      libraries/AP_Motors/AP_MotorsMulticopter.h
  5. 4
      libraries/AP_Motors/AP_MotorsSingle.cpp
  6. 8
      libraries/AP_Motors/AP_MotorsTailsitter.cpp
  7. 6
      libraries/AP_Motors/AP_MotorsTri.cpp

4
libraries/AP_Motors/AP_MotorsCoax.cpp

@ -83,8 +83,8 @@ void AP_MotorsCoax::output_to_motors()
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { 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_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], actuator_spin_up()); set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[6], actuator_spin_up()); set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle());
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break; break;

2
libraries/AP_Motors/AP_MotorsMatrix.cpp

@ -87,7 +87,7 @@ void AP_MotorsMatrix::output_to_motors()
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], actuator_spin_up()); set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
} }
} }
break; break;

4
libraries/AP_Motors/AP_MotorsMulticopter.cpp

@ -437,8 +437,8 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float
actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up); actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
} }
// gradually increase actuator output maximum limit // gradually increase actuator output to spin_min
float AP_MotorsMulticopter::actuator_spin_up() const float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
{ {
return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
} }

4
libraries/AP_Motors/AP_MotorsMulticopter.h

@ -124,8 +124,8 @@ protected:
// adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown // adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown
void set_actuator_with_slew(float& actuator_output, float input); void set_actuator_with_slew(float& actuator_output, float input);
// gradually increase actuator output maximum limit // gradually increase actuator output to ground idle
float actuator_spin_up() const; float actuator_spin_up_to_ground_idle() const;
// apply any thrust compensation for the frame // apply any thrust compensation for the frame
virtual void thrust_compensation(void) {} virtual void thrust_compensation(void) {}

4
libraries/AP_Motors/AP_MotorsSingle.cpp

@ -86,8 +86,8 @@ void AP_MotorsSingle::output_to_motors()
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { 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_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], actuator_spin_up()); set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[6], actuator_spin_up()); set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle());
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break; break;

8
libraries/AP_Motors/AP_MotorsTailsitter.cpp

@ -90,10 +90,10 @@ void AP_MotorsTailsitter::output_to_motors()
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min()); SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min());
break; break;
case GROUND_IDLE: case GROUND_IDLE:
throttle_pwm = output_to_pwm(actuator_spin_up()); throttle_pwm = output_to_pwm(actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[1], actuator_spin_up()); set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(actuator_spin_up())); SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(actuator_spin_up_to_ground_idle()));
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(actuator_spin_up())); SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(actuator_spin_up_to_ground_idle()));
break; break;
case SPOOL_UP: case SPOOL_UP:
case THROTTLE_UNLIMITED: case THROTTLE_UNLIMITED:

6
libraries/AP_Motors/AP_MotorsTri.cpp

@ -87,9 +87,9 @@ void AP_MotorsTri::output_to_motors()
break; break;
case GROUND_IDLE: case GROUND_IDLE:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
set_actuator_with_slew(_actuator[1], actuator_spin_up()); set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[2], actuator_spin_up()); set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[4], actuator_spin_up()); set_actuator_with_slew(_actuator[4], actuator_spin_up_to_ground_idle());
rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1])); rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2])); rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4])); rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));

Loading…
Cancel
Save