mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
APM_Control: added new APM controllers library
See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
This commit is contained in:
Jonathan Challinger
Andrew Tridgell
parent
d45328c98c
commit
10505093e1
3
libraries/APM_Control/APM_Control.h
Normal file
3
libraries/APM_Control/APM_Control.h
Normal file
@ -0,0 +1,3 @@
|
||||
#include "AP_RollController.h"
|
||||
#include "AP_PitchController.h"
|
||||
#include "AP_YawController.h"
|
||||
92
libraries/APM_Control/AP_PitchController.cpp
Normal file
92
libraries/APM_Control/AP_PitchController.cpp
Normal file
@ -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 <math.h>
|
||||
#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;
|
||||
}
|
||||
45
libraries/APM_Control/AP_PitchController.h
Normal file
45
libraries/APM_Control/AP_PitchController.h
Normal file
@ -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 <FastSerial.h>
|
||||
#include <AP_AHRS.h>
|
||||
#include <AP_Common.h>
|
||||
#include <math.h> // 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
|
||||
77
libraries/APM_Control/AP_RollController.cpp
Normal file
77
libraries/APM_Control/AP_RollController.cpp
Normal file
@ -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 <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
43
libraries/APM_Control/AP_RollController.h
Normal file
43
libraries/APM_Control/AP_RollController.h
Normal file
@ -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 <FastSerial.h>
|
||||
#include <AP_AHRS.h>
|
||||
#include <AP_Common.h>
|
||||
#include <math.h> // 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
|
||||
77
libraries/APM_Control/AP_YawController.cpp
Normal file
77
libraries/APM_Control/AP_YawController.cpp
Normal file
@ -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 <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
45
libraries/APM_Control/AP_YawController.h
Normal file
45
libraries/APM_Control/AP_YawController.h
Normal file
@ -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 <FastSerial.h>
|
||||
#include <AP_AHRS.h>
|
||||
#include <AP_Common.h>
|
||||
#include <math.h> // 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
|
||||
Loading…
Reference in New Issue
Block a user