mirror of https://github.com/ArduPilot/ardupilot
78 lines
2.2 KiB
C++
78 lines
2.2 KiB
C++
|
// -*- 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;
|
||
|
|
}
|
||
|
|
|