diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 639bc75073..4f3e903a9a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -18,6 +18,59 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { AP_GROUPEND }; +// +// high level controllers +// + +// init_targets - resets target angles to current angles +void AC_AttitudeControl::init_targets() +{ + _angle_ef_target.x = _ahrs.roll_sensor; + _angle_ef_target.y = _ahrs.pitch_sensor; + _angle_ef_target.z = _ahrs.yaw_sensor; +} + +// angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) +void AC_AttitudeControl::angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef) +{ + // set earth-frame angle targets + _angle_ef_target.x = roll_angle_ef; + _angle_ef_target.y = pitch_angle_ef; + + // convert earth-frame angle targets to earth-frame rate targets + angle_to_rate_ef_roll(); + angle_to_rate_ef_pitch(); + + // set earth-frame rate stabilize target for yaw + _rate_stab_ef_target.z = yaw_rate_ef; + + // convert earth-frame stabilize rate to regular rate target + rate_stab_ef_to_rate_ef_yaw(); + + // convert earth-frame rates to body-frame rates + rate_ef_targets_to_bf(); + + // body-frame to motor outputs should be called separately +} + +// angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame) +void AC_AttitudeControl::angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef) +{ + // set earth-frame angle targets + _angle_ef_target.x = roll_angle_ef; + _angle_ef_target.y = pitch_angle_ef; + + // convert earth-frame angle targets to earth-frame rate targets + angle_to_rate_ef_roll(); + angle_to_rate_ef_pitch(); + angle_to_rate_ef_yaw(); + + // convert earth-frame rates to body-frame rates + rate_ef_targets_to_bf(); + + // body-frame to motor outputs should be called separately +} + // // angle controller methods // @@ -400,4 +453,4 @@ int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm) _angle_boost = throttle_out - throttle_pwm; return throttle_out; -} \ No newline at end of file +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 24bf1b2063..924cfef551 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -73,6 +73,15 @@ public: void set_dt(float delta_sec) { _dt = delta_sec; } float get_dt() { return _dt; } + // init_targets - resets target angles to current angles + void init_targets(); + + // angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) + void angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef); + + // angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame) + void angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef); + // // angle controller (earth-frame) methods //