You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
97 lines
2.9 KiB
97 lines
2.9 KiB
// -*- 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; |
|
}
|
|
|