From 10505093e13d5264f19dd89fdac8d772b7a38c21 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 22 Aug 2012 12:08:14 +1000 Subject: [PATCH] APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details --- libraries/APM_Control/APM_Control.h | 3 + libraries/APM_Control/AP_PitchController.cpp | 92 ++++++++++++++++++++ libraries/APM_Control/AP_PitchController.h | 45 ++++++++++ libraries/APM_Control/AP_RollController.cpp | 77 ++++++++++++++++ libraries/APM_Control/AP_RollController.h | 43 +++++++++ libraries/APM_Control/AP_YawController.cpp | 77 ++++++++++++++++ libraries/APM_Control/AP_YawController.h | 45 ++++++++++ 7 files changed, 382 insertions(+) create mode 100644 libraries/APM_Control/APM_Control.h create mode 100644 libraries/APM_Control/AP_PitchController.cpp create mode 100644 libraries/APM_Control/AP_PitchController.h create mode 100644 libraries/APM_Control/AP_RollController.cpp create mode 100644 libraries/APM_Control/AP_RollController.h create mode 100644 libraries/APM_Control/AP_YawController.cpp create mode 100644 libraries/APM_Control/AP_YawController.h diff --git a/libraries/APM_Control/APM_Control.h b/libraries/APM_Control/APM_Control.h new file mode 100644 index 0000000000..4de57d002c --- /dev/null +++ b/libraries/APM_Control/APM_Control.h @@ -0,0 +1,3 @@ +#include "AP_RollController.h" +#include "AP_PitchController.h" +#include "AP_YawController.h" \ No newline at end of file diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp new file mode 100644 index 0000000000..bec4ac0a5d --- /dev/null +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -0,0 +1,92 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Code by Jon Challinger +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +#include +#include "AP_PitchController.h" + +const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { + AP_GROUPINFO("AP", 0, AP_PitchController, _kp_angle, 1.0), + AP_GROUPINFO("FF", 1, AP_PitchController, _kp_ff, 0.4), + AP_GROUPINFO("RP", 2, AP_PitchController, _kp_rate, 0.0), + AP_GROUPINFO("RI", 3, AP_PitchController, _ki_rate, 0.0), + AP_GROUPINFO("RMAX_U", 4, AP_PitchController, _max_rate_pos, 0.0), + AP_GROUPINFO("RMAX_D", 5, AP_PitchController, _max_rate_neg, 0.0), + AP_GROUPINFO("RLL_FF", 6, AP_PitchController, _roll_ff, 0.0), + AP_GROUPINFO("STAB_GAIN", 7, AP_PitchController, _stabilize_gain, 1), + AP_GROUPEND +}; + +int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize) +{ + uint32_t tnow = millis(); + uint32_t dt = tnow - _last_t; + + if (_last_t == 0 || dt > 1000) { + dt = 0; + } + _last_t = tnow; + + if(_ahrs == NULL) return 0; + float delta_time = (float)dt / 1000.0; + + int8_t inv = 1; + if(abs(_ahrs->roll_sensor)>9000) inv = -1; + + int32_t angle_err = angle - _ahrs->pitch_sensor; + angle_err *= inv; + + float rate = _ahrs->get_pitch_rate_earth(); + + float RC = 1/(2*M_PI*_fCut); + rate = _last_rate + + (delta_time / (RC + delta_time)) * (rate - _last_rate); + _last_rate = rate; + + float roll_scaler = 1/constrain(cos(_ahrs->roll),.33,1); + + int32_t desired_rate = angle_err * _kp_angle; + + if (_max_rate_neg && desired_rate < -_max_rate_neg) desired_rate = -_max_rate_neg; + else if (_max_rate_pos && desired_rate > _max_rate_pos) desired_rate = _max_rate_pos; + + desired_rate *= roll_scaler; + + if(stabilize) { + desired_rate *= _stabilize_gain; + } + + int32_t rate_error = desired_rate - ToDeg(rate)*100; + + float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0); + if(roll_ff > 4500) + roll_ff = 4500; + else if(roll_ff < 0) + roll_ff = 0; + + float out = constrain(((rate_error * _kp_rate) + (desired_rate * _kp_ff) + roll_ff) * scaler,-4500,4500); + + //rate integrator + if (!stabilize) { + if ((fabs(_ki_rate) > 0) && (dt > 0)) + { + _integrator += (rate_error * _ki_rate) * scaler * delta_time; + if (_integrator < -4500-out) _integrator = -4500-out; + else if (_integrator > 4500-out) _integrator = 4500-out; + } + } else { + _integrator = 0; + } + + return out + _integrator; +} + +void AP_PitchController::reset_I() +{ + _integrator = 0; +} diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h new file mode 100644 index 0000000000..3dd6552fbe --- /dev/null +++ b/libraries/APM_Control/AP_PitchController.h @@ -0,0 +1,45 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#ifndef AP_PitchController_h +#define AP_PitchController_h + +#include +#include +#include +#include // for fabs() + +class AP_PitchController { +public: + void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } + + int32_t get_servo_out(int32_t angle, float scaler = 1.0, bool stabilize = false); + + void reset_I(); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_Float _kp_angle; + AP_Float _kp_ff; + AP_Float _kp_rate; + AP_Float _ki_rate; + AP_Int16 _max_rate_pos; + AP_Int16 _max_rate_neg; + AP_Float _roll_ff; + AP_Float _stabilize_gain; + uint32_t _last_t; + float _last_rate; + + float _integrator; + + AP_AHRS *_ahrs; + + /// Low pass filter cut frequency for derivative calculation. + /// + /// 20 Hz becasue anything over that is probably noise, see + /// http://en.wikipedia.org/wiki/Low-pass_filter. + /// + static const uint8_t _fCut = 20; +}; + +#endif diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp new file mode 100644 index 0000000000..89cfcbe82e --- /dev/null +++ b/libraries/APM_Control/AP_RollController.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Code by Jon Challinger +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +#include + +#include "AP_RollController.h" + +const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { + AP_GROUPINFO("AP", 0, AP_RollController, _kp_angle, 1.0), + AP_GROUPINFO("FF", 1, AP_RollController, _kp_ff, 0.4), + AP_GROUPINFO("RP", 2, AP_RollController, _kp_rate, 0.0), + AP_GROUPINFO("RI", 3, AP_RollController, _ki_rate, 0.0), + AP_GROUPINFO("RMAX", 4, AP_RollController, _max_rate, 0), + AP_GROUPINFO("STAB_GAIN", 5, AP_RollController, _stabilize_gain, 1), + AP_GROUPEND +}; + +int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize) +{ + uint32_t tnow = millis(); + uint32_t dt = tnow - _last_t; + if (_last_t == 0 || dt > 1000) { + dt = 0; + } + _last_t = tnow; + + if(_ahrs == NULL) return 0; // can't control without a reference + float delta_time = (float)dt / 1000.0; + + int32_t angle_err = angle - _ahrs->roll_sensor; + + float rate = _ahrs->get_roll_rate_earth(); + + float RC = 1/(2*M_PI*_fCut); + rate = _last_rate + + (delta_time / (RC + delta_time)) * (rate - _last_rate); + _last_rate = rate; + + int32_t desired_rate = angle_err * _kp_angle; + + if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate; + else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate; + + if(stabilize) { + desired_rate *= _stabilize_gain; + } + + int32_t rate_error = desired_rate - ToDeg(rate)*100; + + float out = (rate_error * _kp_rate + desired_rate * _kp_ff) * scaler; + + //rate integrator + if (!stabilize) { + if ((fabs(_ki_rate) > 0) && (dt > 0)) + { + _integrator += (rate_error * _ki_rate) * scaler * delta_time; + if (_integrator < -4500-out) _integrator = -4500-out; + else if (_integrator > 4500-out) _integrator = 4500-out; + } + } else { + _integrator = 0; + } + + return out + _integrator; +} + +void AP_RollController::reset_I() +{ + _integrator = 0; +} + diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h new file mode 100644 index 0000000000..dd2cf8c8f4 --- /dev/null +++ b/libraries/APM_Control/AP_RollController.h @@ -0,0 +1,43 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#ifndef AP_RollController_h +#define AP_RollController_h + +#include +#include +#include +#include // for fabs() + +class AP_RollController { +public: + void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } + + int32_t get_servo_out(int32_t angle, float scaler=1.0, bool stabilize=false); + + void reset_I(); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_Float _kp_angle; + AP_Float _kp_ff; + AP_Float _kp_rate; + AP_Float _ki_rate; + AP_Float _stabilize_gain; + AP_Int16 _max_rate; + uint32_t _last_t; + float _last_rate; + + float _integrator; + + AP_AHRS *_ahrs; + + /// Low pass filter cut frequency for derivative calculation. + /// + /// 20 Hz becasue anything over that is probably noise, see + /// http://en.wikipedia.org/wiki/Low-pass_filter. + /// + static const uint8_t _fCut = 20; +}; + +#endif diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp new file mode 100644 index 0000000000..3e01dce642 --- /dev/null +++ b/libraries/APM_Control/AP_YawController.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Code by Jon Challinger +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +#include + +#include "AP_YawController.h" + +const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { + AP_GROUPINFO("P", 0, AP_YawController, _kp, 0), + AP_GROUPINFO("I", 1, AP_YawController, _ki, 0), + AP_GROUPINFO("IMAX", 2, AP_YawController, _imax, 0), + AP_GROUPEND +}; + +int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) +{ + uint32_t tnow = millis(); + uint32_t dt = tnow - _last_t; + if (_last_t == 0 || dt > 1000) { + dt = 0; + } + _last_t = tnow; + + if(_imu == NULL) { // can't control without a reference + return 0; + } + + float delta_time = (float) dt / 1000.0; + + if(stick_movement) { + if(!_stick_movement) { + _stick_movement_begin = tnow; + } else { + if(_stick_movement_begin < tnow-333) { + _freeze_start_time = tnow; + } + } + } + _stick_movement = stick_movement; + + Vector3f accels = _imu->get_accel(); + + // I didn't pull 512 out of a hat - it is a (very) loose approximation of 100*ToDeg(asin(-accels.y/9.81)) + // which, with a P of 1.0, would mean that your rudder angle would be equal to your roll angle when + // the plane is still. Thus we have an (approximate) unit to go by. + float error = 512 * -accels.y; + + // strongly filter the error + float RC = 1/(2*M_PI*_fCut); + error = _last_error + + (delta_time / (RC + delta_time)) * (error - _last_error); + _last_error = error; + // integrator + if(_freeze_start_time < (tnow - 2000)) { + if ((fabs(_ki) > 0) && (dt > 0)) + { + _integrator += (error * _ki) * scaler * delta_time; + if (_integrator < -_imax) _integrator = -_imax; + else if (_integrator > _imax) _integrator = _imax; + } + } else { + _integrator = 0; + } + + return (error * _kp * scaler) + _integrator; +} + +void AP_YawController::reset_I() +{ + _integrator = 0; +} diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h new file mode 100644 index 0000000000..05833f1791 --- /dev/null +++ b/libraries/APM_Control/AP_YawController.h @@ -0,0 +1,45 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#ifndef AP_YawController_h +#define AP_YawController_h + +#include +#include +#include +#include // for fabs() + +class AP_YawController { +public: + void set_ahrs(AP_AHRS *ahrs) { + _ahrs = ahrs; + _imu = _ahrs->get_imu(); + } + + int32_t get_servo_out(float scaler = 1.0, bool stick_movement = false); + + void reset_I(); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_Float _kp; + AP_Float _ki; + AP_Int16 _imax; + uint32_t _last_t; + float _last_error; + + float _integrator; + bool _stick_movement; + uint32_t _stick_movement_begin; + uint32_t _freeze_start_time; + + AP_AHRS *_ahrs; + IMU *_imu; + + // Low pass filter cut frequency for derivative calculation. + // FCUT macro computes a frequency cut based on an acceptable delay. + #define FCUT(d) (1 / ( 2 * 3.14 * (d) ) ) + static const float _fCut = FCUT(.5); +}; + +#endif