From 715c8eaf020b3cd3982d1c73b19c19d002e7024c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 12 Oct 2013 21:28:18 +0900 Subject: [PATCH] AC_AttitudeControl: first draft of library --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 404 ++++++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.h | 245 +++++++++++ .../AC_AttitudeControl_test.pde | 89 ++++ .../examples/AC_AttitudeControl_test/Makefile | 1 + .../AC_AttitudeControl_test/nocore.inoflag | 0 5 files changed, 739 insertions(+) create mode 100644 libraries/AC_AttitudeControl/AC_AttitudeControl.cpp create mode 100644 libraries/AC_AttitudeControl/AC_AttitudeControl.h create mode 100644 libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde create mode 100644 libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/Makefile create mode 100644 libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/nocore.inoflag diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp new file mode 100644 index 0000000000..eb14ef8ef2 --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -0,0 +1,404 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "AC_AttitudeControl.h" +#include + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { + + // @Param: DUMMY + // @DisplayName: Dummy parameter + // @Description: This is the dummy parameters + // @Increment: 0.1 + // @User: User + AP_GROUPINFO("DUMMY", 0, AC_AttitudeControl, _dummy_param, 0), + + AP_GROUPEND +}; + +// +// angle controller methods +// + +// angle_to_rate_ef_roll - ask the angle controller to calculate the earth frame rate targets for roll +void AC_AttitudeControl::angle_to_rate_ef_roll() +{ + // calculate angle error + // To-Do: is this being converted to int32_t as part of wrap_180_cd? + float angle_error_cd = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); + + // limit the error we're feeding to the PID + // Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits? + // To-Do: move aparm.angle_max into the AP_Vehicles class? + angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX); + + // convert to desired earth-frame rate + // To-Do: replace PI controller with just a single gain? + _rate_ef_target.x = _pi_angle_roll.kP() * angle_error_cd; +} + +// angle_to_rate_ef_pitch - ask the angle controller to calculate the earth frame rate targets for pitch +void AC_AttitudeControl::angle_to_rate_ef_pitch() +{ + // calculate angle error + // To-Do: is this being converted to int32_t as part of wrap_180_cd? + float angle_error_cd = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor); + + // limit the error we're feeding to the PID + // Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits? + // To-Do: move aparm.angle_max into the AP_Vehicles class? + angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX); + + // convert to desired earth-frame rate + // To-Do: replace PI controller with just a single gain? + _rate_ef_target.y = _pi_angle_pitch.kP() * angle_error_cd; +} + +// angle_to_rate_ef_yaw - ask the angle controller to calculate the earth-frame yaw rate in centi-degrees/second +void AC_AttitudeControl::angle_to_rate_ef_yaw() +{ + // calculate angle error + // To-Do: is this being converted to int32_t as part of wrap_180_cd? + float angle_error_cd = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); + + // limit the error we're feeding to the PID + angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX); + + // convert to desired earth-frame rate in centi-degrees/second + // To-Do: replace PI controller with just a single gain? + _rate_ef_target.z = _pi_angle_yaw.kP() * angle_error_cd; + + // To-Do: deal with trad helicopter which do not use yaw rate controllers if using external gyros + + // To-Do: allow logging of PIDs? +} + +// +// stabilized rate controller (earth-frame) methods +// stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers +// because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller +// upon the errors between the actual angle and angle-target. +// +// +// rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis +// targets rates in centi-degrees/second taken from _rate_stab_ef_target +// results in centi-degrees/sec put into _rate_ef_target +void AC_AttitudeControl::rate_stab_ef_to_rate_ef_roll() +{ + float angle_error; + + // convert the input to the desired roll rate + _angle_ef_target.x += _rate_stab_ef_target.x * _dt; + _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); + + // ensure targets are within the lean angle limits + // To-Do: make angle_max part of the AP_Vehicle class + _angle_ef_target.x = constrain_float(_angle_ef_target.x, -aparm.angle_max, aparm.angle_max); + + // calculate angle error with maximum of +- max_angle_overshoot + angle_error = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); + angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle check for traditional heli's motors.motor_runup_complete + // To-Do: reset target angle to current angle if motors not spinning + + // update acro_roll to be within max_angle_overshoot of our current heading + _angle_ef_target.x = wrap_180_cd(angle_error + _ahrs.roll_sensor); + + // set earth frame rate controller targets + _rate_ef_target.x = _pi_angle_roll.get_p(angle_error) + _rate_stab_ef_target.x; +} + +void AC_AttitudeControl::rate_stab_ef_to_rate_ef_pitch() +{ + float angle_error; + + // convert the input to the desired roll rate + _angle_ef_target.y += _rate_stab_ef_target.y * _dt; + _angle_ef_target.y = wrap_180_cd(_angle_ef_target.y); + + // ensure targets are within the lean angle limits + // To-Do: make angle_max part of the AP_Vehicle class + _angle_ef_target.y = constrain_float(_angle_ef_target.y, -aparm.angle_max, aparm.angle_max); + + // calculate angle error with maximum of +- max_angle_overshoot + // To-Do: should we do something better as we cross 90 degrees? + angle_error = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor); + angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle check for traditional heli's motors.motor_runup_complete + // To-Do: reset target angle to current angle if motors not spinning + + // update acro_roll to be within max_angle_overshoot of our current heading + _angle_ef_target.y = wrap_180_cd(angle_error + _ahrs.pitch_sensor); + + // set earth frame rate controller targets + _rate_ef_target.y = _pi_angle_pitch.get_p(angle_error) + _rate_stab_ef_target.y; +} + +void AC_AttitudeControl::rate_stab_ef_to_rate_ef_yaw() +{ + float angle_error; + + // convert the input to the desired roll rate + _angle_ef_target.z += _rate_stab_ef_target.z * _dt; + _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z); + + // calculate angle error with maximum of +- max_angle_overshoot + angle_error = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); + angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle check for traditional heli's motors.motor_runup_complete + // To-Do: reset target angle to current angle if motors not spinning + + // update acro_roll to be within max_angle_overshoot of our current heading + _angle_ef_target.z = wrap_360_cd(angle_error + _ahrs.yaw_sensor); + + // set earth frame rate controller targets + _rate_ef_target.z = _pi_angle_yaw.get_p(angle_error) + _rate_stab_ef_target.z; +} + +// +// stabilized rate controller (body-frame) methods +// + +// rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis +// targets rates in centi-degrees/second taken from _rate_stab_bf_target +// results in centi-degrees/sec put into _rate_bf_target +void AC_AttitudeControl::rate_stab_bf_to_rate_bf_roll() +{ + // calculate rate correction from angle errors + // To-Do: do we still need to have this rate correction calculated from the previous iteration's errors? + float rate_correction = _pi_angle_roll.get_p(_rate_stab_bf_error.x); + + // set body frame targets for rate controller + _rate_bf_target.x = _rate_stab_bf_target.x + rate_correction; + + // calculate body-frame angle error by integrating body-frame rate error + _rate_stab_bf_error.x += (_rate_stab_bf_target.x - (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + + // limit maximum error + _rate_stab_bf_error.x = constrain_float(_rate_stab_bf_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero +} + +void AC_AttitudeControl::rate_stab_bf_to_rate_bf_pitch() +{ + // calculate rate correction from angle errors + // To-Do: do we still need to have this rate correction calculated from the previous iteration's errors? + float rate_correction = _pi_angle_pitch.get_p(_rate_stab_bf_error.y); + + // set body frame targets for rate controller + _rate_bf_target.y = _rate_stab_bf_target.y + rate_correction; + + // calculate body-frame angle error by integrating body-frame rate error + _rate_stab_bf_error.y += (_rate_stab_bf_target.y - (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + + // limit maximum error + _rate_stab_bf_error.y = constrain_float(_rate_stab_bf_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero +} + +void AC_AttitudeControl::rate_stab_bf_to_rate_bf_yaw() +{ + // calculate rate correction from angle errors + float rate_correction = _pi_angle_yaw.get_p(_rate_stab_bf_error.z); + + // set body frame targets for rate controller + _rate_bf_target.y = _rate_stab_bf_target.y + rate_correction; + + // calculate body-frame angle error by integrating body-frame rate error + _rate_stab_bf_error.z += (_rate_stab_bf_target.z - (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + + // limit maximum error + _rate_stab_bf_error.z = constrain_float(_rate_stab_bf_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); + + // To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero +} + +// +// rate controller (earth-frame) methods +// + +// rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets +void AC_AttitudeControl::rate_ef_targets_to_bf() +{ + // convert earth frame rates to body frame rates + _rate_bf_target.x = _rate_ef_target.x - _sin_pitch * _rate_ef_target.z; + _rate_bf_target.y = _cos_roll * _rate_ef_target.y + _sin_roll * _cos_pitch * _rate_ef_target.z; + _rate_bf_target.z = _cos_pitch * _cos_roll * _rate_ef_target.z - _sin_roll * _rate_ef_target.y; +} + +// +// rate controller (body-frame) methods +// + +// rate_controller_run - run lowest level rate controller and send outputs to the motors +// should be called at 100hz or more +void AC_AttitudeControl::rate_controller_run() +{ + // call rate controllers and send output to motors object + // To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library? + // To-Do: skip this step if the throttle out is zero? + _motor_roll = rate_bf_to_motor_roll(_rate_bf_target.x); + _motor_pitch = rate_bf_to_motor_pitch(_rate_bf_target.y); + _motor_yaw = rate_bf_to_motor_yaw(_rate_bf_target.z); + + // To-Do: what about throttle? + //_motor_ +} + +// +// private methods +// +// +// body-frame rate controller +// + +// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second +float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds) +{ + float p,i,d; // used to capture pid values for logging + float current_rate; // this iteration's rate + float rate_error; // simply target_rate - current_rate + + // get current rate + // To-Do: make getting gyro rates more efficient? + current_rate = (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100); + + // calculate error and call pid controller + rate_error = rate_target_cds - current_rate; + p = _pid_rate_roll.get_p(rate_error); + + // get i term + i = _pid_rate_roll.get_integrator(); + + // update i term as long as we haven't breached the limits or the I term will certainly reduce + if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) { + i = _pid_rate_roll.get_i(rate_error, _dt); + } + + // get d term + d = _pid_rate_roll.get_d(rate_error, _dt); + + // constrain output and return + return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); + + // To-Do: allow logging of PIDs? +} + +// rate_bf_to_motor_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second +float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds) +{ + float p,i,d; // used to capture pid values for logging + float current_rate; // this iteration's rate + float rate_error; // simply target_rate - current_rate + + // get current rate + // To-Do: make getting gyro rates more efficient? + current_rate = (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100); + + // calculate error and call pid controller + rate_error = rate_target_cds - current_rate; + p = _pid_rate_pitch.get_p(rate_error); + + // get i term + i = _pid_rate_pitch.get_integrator(); + + // update i term as long as we haven't breached the limits or the I term will certainly reduce + if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) { + i = _pid_rate_pitch.get_i(rate_error, _dt); + } + + // get d term + d = _pid_rate_pitch.get_d(rate_error, _dt); + + // constrain output and return + return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); + + // To-Do: allow logging of PIDs? +} + +// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second +float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds) +{ + float p,i,d; // used to capture pid values for logging + float current_rate; // this iteration's rate + float rate_error; // simply target_rate - current_rate + + // get current rate + // To-Do: make getting gyro rates more efficient? + current_rate = (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100); + + // calculate error and call pid controller + rate_error = rate_target_cds - current_rate; + p = _pid_rate_yaw.get_p(rate_error); + + // separately calculate p, i, d values for logging + p = _pid_rate_yaw.get_p(rate_error); + + // get i term + i = _pid_rate_yaw.get_integrator(); + + // update i term as long as we haven't breached the limits or the I term will certainly reduce + if (!_motors.limit.yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) { + i = _pid_rate_yaw.get_i(rate_error, _dt); + } + + // get d value + d = _pid_rate_yaw.get_d(rate_error, _dt); + + // constrain output and return + return constrain_float((p+i+d), -AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX); + + // To-Do: allow logging of PIDs? +} + + +// +// throttle functions +// + + // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors + // provide 0 to cut motors +void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost) +{ + if( apply_angle_boost ) { + _motor_throttle = get_angle_boost(throttle_out); + }else{ + _motor_throttle = throttle_out; + // clear angle_boost for logging purposes + //angle_boost = 0; + } + + // update compass with throttle value + // To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class + //compass.set_throttle((float)g.rc_3.servo_out/1000.0f); +} + +// get_angle_boost - returns a throttle including compensation for roll/pitch angle +// throttle value should be 0 ~ 1000 +int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm) const +{ + float temp = _cos_pitch * _cos_roll; + int16_t throttle_out; + + temp = constrain_float(temp, 0.5f, 1.0f); + + // reduce throttle if we go inverted + temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp); + + // apply scale and constrain throttle + // To-Do: move throttle_min and throttle_max into the AP_Vehicles class? + //throttle_out = constrain_float((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000); + + // to allow logging of angle boost + // To-Do: expose this so it can be logged? + //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 new file mode 100644 index 0000000000..f9f9c4d9ab --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -0,0 +1,245 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_AttitudeControl.h +/// @brief ArduCopter attitude control library + +#ifndef AC_AttitudeControl_H +#define AC_AttitudeControl_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// To-Do: change the name or move to AP_Math? +#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees +#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds +#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 5000.0f // body-frame rate controller maximum output (for roll-pitch axis) +#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis) +#define AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX 4500.0f // earth-frame angle controller maximum output (for yaw axis) +#define AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX 4500.0f // earth-frame angle controller maximum input angle (To-Do: replace with reference to aparm.angle_max) + +#define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle +#define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle +#define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX 1000.0f // earth-frame rate stabilize controller's maximum overshoot angle + +#define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate +#define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate + +class AC_AttitudeControl { +public: + AC_AttitudeControl( AP_AHRS &ahrs, + AP_InertialSensor& ins, + const AP_Vehicle::MultiCopter &parms, + AP_Motors& motors, + APM_PI& pi_angle_roll, APM_PI& pi_angle_pitch, APM_PI& pi_angle_yaw, + AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw, + int16_t& motor_roll, int16_t& motor_pitch, int16_t& motor_yaw, int16_t& motor_throttle + ) : + _ahrs(ahrs), + _ins(ins), + aparm(parms), + _motors(motors), + _pi_angle_roll(pi_angle_roll), + _pi_angle_pitch(pi_angle_pitch), + _pi_angle_yaw(pi_angle_yaw), + _pid_rate_roll(pid_rate_roll), + _pid_rate_pitch(pid_rate_pitch), + _pid_rate_yaw(pid_rate_yaw), + _motor_roll(motor_roll), + _motor_pitch(motor_pitch), + _motor_yaw(motor_yaw), + _motor_throttle(motor_throttle), + _dt(AC_ATTITUDE_100HZ_DT) + { + AP_Param::setup_object_defaults(this, var_info); + } + + // + // initialisation functions + // + + // set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) + void set_dt(float delta_sec) { _dt = delta_sec; } + float get_dt() { return _dt; } + + // + // angle controller (earth-frame) methods + // + + // angle_ef_targets - get and set angle controller earth-frame-targets + Vector3f angle_ef_targets() const { return _angle_ef_target; } + void angle_ef_targets(Vector3f& angle_cd) { _angle_ef_target = angle_cd; } + + // angle_to_rate_ef_roll - converts earth-frame angle targets to earth-frame rate targets for roll, pitch and yaw axis + // targets angles in centi-degrees should be placed in _angle_ef_targets + // results in centi-degrees/sec put directly into _rate_ef_target + void angle_to_rate_ef_roll(); + void angle_to_rate_ef_pitch(); + void angle_to_rate_ef_yaw(); + + // + // stabilized rate controller (earth-frame) methods + // + + // rate_stab_ef_targets - gets and sets the stabilized rate controller earth-frame targets + // stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers + // because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller + // upon the errors between the actual angle and angle-target. + // + Vector3f rate_stab_ef_targets() const { return _rate_stab_ef_target; } + void rate_stab_ef_targets(Vector3f& rates_cds) { _rate_stab_ef_target = rates_cds; } + + // rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis + // targets rates in centi-degrees/second taken from _rate_stab_ef_target + // results in centi-degrees/sec put into _rate_ef_target + void rate_stab_ef_to_rate_ef_roll(); + void rate_stab_ef_to_rate_ef_pitch(); + void rate_stab_ef_to_rate_ef_yaw(); + + // + // stabilized rate controller (body-frame) methods + // + + // rate_stab_bf_targets - gets and sets the stabilized rate controller body-frame targets + Vector3f rate_stab_bf_targets() const { return _rate_stab_bf_target; } + void rate_stab_bf_targets(Vector3f& rates_cds) { _rate_stab_bf_target = rates_cds; } + + // rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis + // targets rates in centi-degrees/second taken from _rate_stab_bf_target + // results in centi-degrees/sec put into _rate_bf_target + void rate_stab_bf_to_rate_bf_roll(); + void rate_stab_bf_to_rate_bf_pitch(); + void rate_stab_bf_to_rate_bf_yaw(); + + // + // rate controller (earth-frame) methods + // + + // rate_ef_targets - gets and sets rate controller earth-frame-targets + Vector3f rate_ef_targets() const { return _rate_ef_target; } + void rate_ef_targets(Vector3f& rates_cds) { _rate_ef_target = rates_cds; } + + // rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets + void rate_ef_targets_to_bf(); + + + // + // rate controller (body-frame) methods + // + + // rate_bf_targets - gets and sets rate controller body-frame targets + // To-Do: can we return these Vector3fs by reference? i.e. using "&"? + Vector3f rate_bf_targets() const { return _rate_bf_target; } + void rate_bf_targets(Vector3f& rates_cds) { _rate_bf_target = rates_cds; } + + // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors + // should be called at 100hz or more + void rate_controller_run(); + + // + // throttle functions + // + + // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors + // provide 0 to cut motors + void set_throttle_out(int16_t throttle_pwm, bool apply_angle_boost); + + // + // helper functions + // + + /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame + /// To-Do: make these references or calculate in ahrs + void set_cos_sin_yaw(float cos_roll, float cos_pitch, float sin_roll, float sin_pitch) { + _cos_roll = cos_roll; + _cos_pitch = cos_pitch; + _sin_roll = sin_roll; + _sin_pitch = sin_pitch; + } + + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // + // body-frame rate controller + // + // rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in centi-degrees/sec) for roll, pitch and yaw + float rate_bf_to_motor_roll(float rate_target_cds); + float rate_bf_to_motor_pitch(float rate_target_cds); + float rate_bf_to_motor_yaw(float rate_target_cds); + + // + // throttle methods + // + + // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle + int16_t get_angle_boost(int16_t throttle_pwm) const; + + // + // logging + // + + // log data on internal state of the controller. Called at 10Hz + void log_data(DataFlash_Class &dataflash, uint8_t msgid); + + // dataflash logging packet + struct PACKED log_Attitude { + LOG_PACKET_HEADER; + int16_t roll_in; + int16_t roll; + int16_t pitch_in; + int16_t pitch; + int16_t yaw_in; + uint16_t yaw; + uint16_t nav_yaw; + } log_ACAttControl; + + // references to external libraries + AP_AHRS& _ahrs; + AP_InertialSensor& _ins; + const AP_Vehicle::MultiCopter &aparm; + AP_Motors& _motors; + APM_PI& _pi_angle_roll; + APM_PI& _pi_angle_pitch; + APM_PI& _pi_angle_yaw; + AC_PID& _pid_rate_roll; + AC_PID& _pid_rate_pitch; + AC_PID& _pid_rate_yaw; + int16_t& _motor_roll; // g.rc_1.servo_out + int16_t& _motor_pitch; // g.rc_2.servo_out + int16_t& _motor_yaw; // g.rc_4.servo_out + int16_t& _motor_throttle; // g.rc_3.servo_out + + // parameters + AP_Float _dummy_param; + + // internal variables + // To-Do: make rate targets a typedef instead of Vector3f? + float _dt; // time delta in seconds + Vector3f _angle_ef_target; // angle controller earth-frame targets + Vector3f _rate_stab_ef_target; // stabilized rate controller earth-frame rate targets + Vector3f _rate_ef_target; // rate controller earth-frame targets + Vector3f _rate_stab_bf_target; // stabilized rate controller body-frame rate targets + Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors + Vector3f _rate_bf_target; // rate controller body-frame targets + + // precalculated values for efficiency saves + // To-Do: could these be changed to references and passed into the constructor? + float _cos_roll; + float _cos_pitch; + float _sin_roll; + float _sin_pitch; +}; + +#define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \ + "ATT", "cccccCC", "RollIn,Roll,PitchIn,Pitch,YawIn,Yaw,NavYaw" } + +#endif //AC_AttitudeControl_H diff --git a/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde new file mode 100644 index 0000000000..52698dfd86 --- /dev/null +++ b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde @@ -0,0 +1,89 @@ +/* + * Example of AC_AttitudeControl library + * DIYDrones.com + */ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust +#include +#include +#include + +#include // ArduPilot GPS library +#include // GPS glitch protection library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include // ArduPilot Mega Barometer Library +#include +#include // ArduPilot Mega Magnetometer Library +#include +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include +#include +#include // PID library +#include // PID library +#include // ArduPilot general purpose FIFO buffer +#include // Inertial Navigation library +#include +#include +#include +#include +#include // RC Channel Library +#include +#include + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +// INS and Baro declaration +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + +AP_InertialSensor_MPU6000 ins; +AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi); + +#else + +AP_ADC_ADS7844 adc; +AP_InertialSensor_Oilpan ins(&adc); +AP_Baro_BMP085 baro; +#endif + +// GPS declaration +GPS *gps; +AP_GPS_Auto auto_gps(&gps); +GPS_Glitch gps_glitch(gps); + +AP_Compass_HMC5843 compass; +AP_AHRS_DCM ahrs(&ins, gps); + +// Inertial Nav declaration +AP_InertialNav inertial_nav(&ahrs, &ins, &baro, gps, gps_glitch); + +// fake PIDs +APM_PI pi_angle_roll, pi_angle_pitch, pi_angle_yaw; +AC_PID pid_rate_roll, pid_rate_pitch, pid_rate_yaw; + +// fake RC inputs +RC_Channel rc_roll(CH_1), rc_pitch(CH_2), rc_yaw(CH_4), rc_throttle(CH_3); + +// fake motor and outputs +AP_MotorsQuad motors(&rc_roll, &rc_pitch, &rc_throttle, &rc_yaw); +int16_t motor_roll, motor_pitch, motor_yaw, motor_throttle; + +// Attitude Control +AC_AttitudeControl ac_control(ahrs, ins, motors, pi_angle_roll, pi_angle_pitch, pi_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw, motor_roll, motor_pitch, motor_yaw, motor_throttle); + +void setup() +{ + hal.console->println("AC_AttitudeControl library test"); +} + +void loop() +{ + // call update function + hal.console->printf_P(PSTR("hello")); + hal.scheduler->delay(1); +} + +AP_HAL_MAIN(); diff --git a/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/Makefile b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/Makefile new file mode 100644 index 0000000000..f5daf25151 --- /dev/null +++ b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/Makefile @@ -0,0 +1 @@ +include ../../../../mk/apm.mk diff --git a/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/nocore.inoflag b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2