mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
205397d030
The previous calculation constrained the speed used to calculate the bank compensation rate offset between the min and max fbw speeds. This would result in an unwanted climb if flown above the max fbw speed (this could happen in fbw-a mode)
98 lines
2.9 KiB
C++
98 lines
2.9 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 <AP_Math.h>
|
|
#include <AP_HAL.h>
|
|
#include "AP_YawController.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
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
|
|
};
|
|
|
|
// 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.14f * (d) ) )
|
|
const float AP_YawController::_fCut = FCUT(0.5f);
|
|
|
|
int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
|
|
{
|
|
uint32_t tnow = hal.scheduler->millis();
|
|
uint32_t dt = tnow - _last_t;
|
|
if (_last_t == 0 || dt > 1000) {
|
|
dt = 0;
|
|
}
|
|
_last_t = tnow;
|
|
|
|
if(_ins == NULL) { // can't control without a reference
|
|
return 0;
|
|
}
|
|
|
|
float delta_time = (float) dt / 1000.0f;
|
|
|
|
if(stick_movement) {
|
|
if(!_stick_movement) {
|
|
_stick_movement_begin = tnow;
|
|
} else {
|
|
if(_stick_movement_begin < tnow-333) {
|
|
_freeze_start_time = tnow;
|
|
}
|
|
}
|
|
}
|
|
rate_offset = (9.807f / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
|
|
|
|
// Get body rate vector (radians/sec)
|
|
float omega_z = _ahrs->get_gyro().z;
|
|
|
|
// Subtract the steady turn component of rate from the measured rate
|
|
// to calculate the rate relative to the turn requirement in degrees/sec
|
|
float rate_hp_in = ToDeg(omega_z - rate_offset);
|
|
|
|
// Apply a high-pass filter to the rate to washout any steady state error
|
|
// due to bias errors in rate_offset
|
|
// Use a cut-off frequency of omega = 0.2 rad/sec
|
|
// Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
|
|
float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;
|
|
_last_rate_hp_out = rate_hp_out;
|
|
_last_rate_hp_in = rate_hp_in;
|
|
|
|
// Get the accln vector (m/s^2)
|
|
Vector3f accel = _ins->get_accel();
|
|
|
|
// Calculate input to integrator
|
|
float integ_in = - _K_I * (_K_A * accel.y + rate_hp_out);
|
|
|
|
// strongly filter the error
|
|
float RC = 1/(2*PI*_fCut);
|
|
error = _last_error +
|
|
(delta_time / (RC + delta_time)) * (error - _last_error);
|
|
_last_error = error;
|
|
// integrator
|
|
if(_freeze_start_time < (tnow - 2000)) {
|
|
if ((fabsf(_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;
|
|
}
|