Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: ported to AP_HAL

Browse Source 
Unable to test since there are no unit tests.
This commit is contained in:
Pat Hickey 2012-10-12 10:53:42 -07:00 committed by Andrew Tridgell
parent 9f9dfc7c63
commit 95a13bdbd2
6 changed files with 31 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/APM_Control/AP_PitchController.cpp
View File

@ -8,8 +8,11 @@
// version 2.1 of the License, or (at your option) any later version. // version 2.1 of the License, or (at your option) any later version.
#include <math.h> #include <math.h>
#include <AP_HAL.h>
#include "AP_PitchController.h" #include "AP_PitchController.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
AP_GROUPINFO("AP", 0, AP_PitchController, _kp_angle, 1.0), AP_GROUPINFO("AP", 0, AP_PitchController, _kp_angle, 1.0),
AP_GROUPINFO("FF", 1, AP_PitchController, _kp_ff, 0.4), AP_GROUPINFO("FF", 1, AP_PitchController, _kp_ff, 0.4),
@ -24,7 +27,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize) int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize)
{ {
uint32_t tnow = millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) { if (_last_t == 0 || dt > 1000) {
@ -52,8 +55,11 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
int32_t desired_rate = angle_err * _kp_angle; int32_t desired_rate = angle_err * _kp_angle;
if (_max_rate_neg && desired_rate < -_max_rate_neg) desired_rate = -_max_rate_neg; if (_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 = -_max_rate_neg;
} else if (_max_rate_pos && desired_rate > _max_rate_pos) {
desired_rate = _max_rate_pos;
}
desired_rate *= roll_scaler; desired_rate *= roll_scaler;

7
libraries/APM_Control/AP_PitchController.h
View File

@ -1,9 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_PitchController_h #ifndef __AP_PITCH_CONTROLLER_H__
#define AP_PitchController_h #define __AP_PITCH_CONTROLLER_H__
#include <FastSerial.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
@ -42,4 +41,4 @@ private:
static const uint8_t _fCut = 20; static const uint8_t _fCut = 20;
}; };
#endif #endif // __AP_PITCH_CONTROLLER_H__

5
libraries/APM_Control/AP_RollController.cpp
View File

@ -8,9 +8,12 @@
// version 2.1 of the License, or (at your option) any later version. // version 2.1 of the License, or (at your option) any later version.
#include <math.h> #include <math.h>
#include <AP_HAL.h>
#include "AP_RollController.h" #include "AP_RollController.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
AP_GROUPINFO("AP", 0, AP_RollController, _kp_angle, 1.0), AP_GROUPINFO("AP", 0, AP_RollController, _kp_angle, 1.0),
AP_GROUPINFO("FF", 1, AP_RollController, _kp_ff, 0.4), AP_GROUPINFO("FF", 1, AP_RollController, _kp_ff, 0.4),
@ -23,7 +26,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize) int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize)
{ {
uint32_t tnow = millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) { if (_last_t == 0 || dt > 1000) {
dt = 0; dt = 0;

7
libraries/APM_Control/AP_RollController.h
View File

@ -1,9 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_RollController_h #ifndef __AP_ROLL_CONTROLLER_H__
#define AP_RollController_h #define __AP_ROLL_CONTROLLER_H__
#include <FastSerial.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
@ -40,4 +39,4 @@ private:
static const uint8_t _fCut = 20; static const uint8_t _fCut = 20;
}; };
#endif #endif // __AP_ROLL_CONTROLLER_H__

15
libraries/APM_Control/AP_YawController.cpp
View File

@ -8,9 +8,11 @@
// version 2.1 of the License, or (at your option) any later version. // version 2.1 of the License, or (at your option) any later version.
#include <math.h> #include <math.h>
#include <AP_HAL.h>
#include "AP_YawController.h" #include "AP_YawController.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, AP_YawController, _kp, 0), AP_GROUPINFO("P", 0, AP_YawController, _kp, 0),
AP_GROUPINFO("I", 1, AP_YawController, _ki, 0), AP_GROUPINFO("I", 1, AP_YawController, _ki, 0),
@ -20,7 +22,7 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = {
int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
{ {
uint32_t tnow = millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) { if (_last_t == 0 || dt > 1000) {
dt = 0; dt = 0;
@ -46,8 +48,10 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
Vector3f accels = _ins->get_accel(); Vector3f accels = _ins->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)) // I didn't pull 512 out of a hat - it is a (very) loose approximation of
// which, with a P of 1.0, would mean that your rudder angle would be equal to your roll angle when // 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. // the plane is still. Thus we have an (approximate) unit to go by.
float error = 512 * -accels.y; float error = 512 * -accels.y;
@ -58,8 +62,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
_last_error = error; _last_error = error;
// integrator // integrator
if(_freeze_start_time < (tnow - 2000)) { if(_freeze_start_time < (tnow - 2000)) {
if ((fabs(_ki) > 0) && (dt > 0)) if ((fabs(_ki) > 0) && (dt > 0)) {
{
_integrator += (error * _ki) * scaler * delta_time; _integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) _integrator = -_imax; if (_integrator < -_imax) _integrator = -_imax;
else if (_integrator > _imax) _integrator = _imax; else if (_integrator > _imax) _integrator = _imax;

7
libraries/APM_Control/AP_YawController.h
View File

@ -1,9 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_YawController_h #ifndef __AP_YAW_CONTROLLER_H__
#define AP_YawController_h #define __AP_YAW_CONTROLLER_H__
#include <FastSerial.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
@ -42,4 +41,4 @@ private:
static const float _fCut = FCUT(.5); static const float _fCut = FCUT(.5);
}; };
#endif #endif // __AP_YAW_CONTROLLER_H__