Browse Source

AP_MotorsHeli: Move set_delta_phase_angle into _Single class

mission-4.1.18
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
commit
b11600aebe
  1. 9
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 6
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 9
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp
  4. 4
      libraries/AP_Motors/AP_MotorsHeli_Single.h

9
libraries/AP_Motors/AP_MotorsHeli.cpp

@ -288,15 +288,6 @@ void AP_MotorsHeli::init_swash_servo(RC_Channel& servo) @@ -288,15 +288,6 @@ void AP_MotorsHeli::init_swash_servo(RC_Channel& servo)
servo.radio_max = 2000;
}
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void AP_MotorsHeli::set_delta_phase_angle(int16_t angle)
{
angle = constrain_int16(angle, -90, 90);
_delta_phase_angle = angle;
calculate_roll_pitch_collective_factors();
}
// update the throttle input filter
void AP_MotorsHeli::update_throttle_filter()
{

6
libraries/AP_Motors/AP_MotorsHeli.h

@ -150,9 +150,9 @@ public: @@ -150,9 +150,9 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void set_delta_phase_angle(int16_t angle);
// set_delta_phase_angle for setting variable phase angle compensation and force recalculation of collective factors
// ignored unless overloaded by child classes
virtual void set_delta_phase_angle(int16_t angle){};
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict

9
libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@ -428,6 +428,15 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() @@ -428,6 +428,15 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
}
}
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle)
{
angle = constrain_int16(angle, -90, 90);
_delta_phase_angle = angle;
calculate_roll_pitch_collective_factors();
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:

4
libraries/AP_Motors/AP_MotorsHeli_Single.h

@ -121,6 +121,10 @@ public: @@ -121,6 +121,10 @@ public:
// supports_yaw_passthrought - returns true if we support yaw passthrough
bool supports_yaw_passthrough() const { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; }
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void set_delta_phase_angle(int16_t angle);
// var_info
static const struct AP_Param::GroupInfo var_info[];

Loading…
Cancel
Save