Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: first draft of library

This commit is contained in:
Randy Mackay 2013-10-12 21:28:18 +09:00 committed by Andrew Tridgell
parent 6029ec53ae
commit 715c8eaf02
5 changed files with 739 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

404
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp Normal file
View File

@ -0,0 +1,404 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include "AC_AttitudeControl.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
// @Param: DUMMY
// @DisplayName: Dummy parameter
// @Description: This is the dummy parameters
// @Increment: 0.1
// @User: User
AP_GROUPINFO("DUMMY", 0, AC_AttitudeControl, _dummy_param, 0),
AP_GROUPEND
};
//
// angle controller methods
//
// angle_to_rate_ef_roll - ask the angle controller to calculate the earth frame rate targets for roll
void AC_AttitudeControl::angle_to_rate_ef_roll()
{
// calculate angle error
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
// limit the error we're feeding to the PID
// Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits?
// To-Do: move aparm.angle_max into the AP_Vehicles class?
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX);
// convert to desired earth-frame rate
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.x = _pi_angle_roll.kP() * angle_error_cd;
}
// angle_to_rate_ef_pitch - ask the angle controller to calculate the earth frame rate targets for pitch
void AC_AttitudeControl::angle_to_rate_ef_pitch()
{
// calculate angle error
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
// limit the error we're feeding to the PID
// Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits?
// To-Do: move aparm.angle_max into the AP_Vehicles class?
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX);
// convert to desired earth-frame rate
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.y = _pi_angle_pitch.kP() * angle_error_cd;
}
// angle_to_rate_ef_yaw - ask the angle controller to calculate the earth-frame yaw rate in centi-degrees/second
void AC_AttitudeControl::angle_to_rate_ef_yaw()
{
// calculate angle error
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
// limit the error we're feeding to the PID
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX);
// convert to desired earth-frame rate in centi-degrees/second
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.z = _pi_angle_yaw.kP() * angle_error_cd;
// To-Do: deal with trad helicopter which do not use yaw rate controllers if using external gyros
// To-Do: allow logging of PIDs?
}
//
// stabilized rate controller (earth-frame) methods
// stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers
// because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller
// upon the errors between the actual angle and angle-target.
//
//
// rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees/second taken from _rate_stab_ef_target
// results in centi-degrees/sec put into _rate_ef_target
void AC_AttitudeControl::rate_stab_ef_to_rate_ef_roll()
{
float angle_error;
// convert the input to the desired roll rate
_angle_ef_target.x += _rate_stab_ef_target.x * _dt;
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
// ensure targets are within the lean angle limits
// To-Do: make angle_max part of the AP_Vehicle class
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -aparm.angle_max, aparm.angle_max);
// calculate angle error with maximum of +- max_angle_overshoot
angle_error = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// To-Do: handle check for traditional heli's motors.motor_runup_complete
// To-Do: reset target angle to current angle if motors not spinning
// update acro_roll to be within max_angle_overshoot of our current heading
_angle_ef_target.x = wrap_180_cd(angle_error + _ahrs.roll_sensor);
// set earth frame rate controller targets
_rate_ef_target.x = _pi_angle_roll.get_p(angle_error) + _rate_stab_ef_target.x;
}
void AC_AttitudeControl::rate_stab_ef_to_rate_ef_pitch()
{
float angle_error;
// convert the input to the desired roll rate
_angle_ef_target.y += _rate_stab_ef_target.y * _dt;
_angle_ef_target.y = wrap_180_cd(_angle_ef_target.y);
// ensure targets are within the lean angle limits
// To-Do: make angle_max part of the AP_Vehicle class
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -aparm.angle_max, aparm.angle_max);
// calculate angle error with maximum of +- max_angle_overshoot
// To-Do: should we do something better as we cross 90 degrees?
angle_error = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
// To-Do: handle check for traditional heli's motors.motor_runup_complete
// To-Do: reset target angle to current angle if motors not spinning
// update acro_roll to be within max_angle_overshoot of our current heading
_angle_ef_target.y = wrap_180_cd(angle_error + _ahrs.pitch_sensor);
// set earth frame rate controller targets
_rate_ef_target.y = _pi_angle_pitch.get_p(angle_error) + _rate_stab_ef_target.y;
}
void AC_AttitudeControl::rate_stab_ef_to_rate_ef_yaw()
{
float angle_error;
// convert the input to the desired roll rate
_angle_ef_target.z += _rate_stab_ef_target.z * _dt;
_angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);
// calculate angle error with maximum of +- max_angle_overshoot
angle_error = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
angle_error = constrain_float(angle_error, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
// To-Do: handle check for traditional heli's motors.motor_runup_complete
// To-Do: reset target angle to current angle if motors not spinning
// update acro_roll to be within max_angle_overshoot of our current heading
_angle_ef_target.z = wrap_360_cd(angle_error + _ahrs.yaw_sensor);
// set earth frame rate controller targets
_rate_ef_target.z = _pi_angle_yaw.get_p(angle_error) + _rate_stab_ef_target.z;
}
//
// stabilized rate controller (body-frame) methods
//
// rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees/second taken from _rate_stab_bf_target
// results in centi-degrees/sec put into _rate_bf_target
void AC_AttitudeControl::rate_stab_bf_to_rate_bf_roll()
{
// calculate rate correction from angle errors
// To-Do: do we still need to have this rate correction calculated from the previous iteration's errors?
float rate_correction = _pi_angle_roll.get_p(_rate_stab_bf_error.x);
// set body frame targets for rate controller
_rate_bf_target.x = _rate_stab_bf_target.x + rate_correction;
// calculate body-frame angle error by integrating body-frame rate error
_rate_stab_bf_error.x += (_rate_stab_bf_target.x - (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
// limit maximum error
_rate_stab_bf_error.x = constrain_float(_rate_stab_bf_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero
}
void AC_AttitudeControl::rate_stab_bf_to_rate_bf_pitch()
{
// calculate rate correction from angle errors
// To-Do: do we still need to have this rate correction calculated from the previous iteration's errors?
float rate_correction = _pi_angle_pitch.get_p(_rate_stab_bf_error.y);
// set body frame targets for rate controller
_rate_bf_target.y = _rate_stab_bf_target.y + rate_correction;
// calculate body-frame angle error by integrating body-frame rate error
_rate_stab_bf_error.y += (_rate_stab_bf_target.y - (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
// limit maximum error
_rate_stab_bf_error.y = constrain_float(_rate_stab_bf_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
// To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero
}
void AC_AttitudeControl::rate_stab_bf_to_rate_bf_yaw()
{
// calculate rate correction from angle errors
float rate_correction = _pi_angle_yaw.get_p(_rate_stab_bf_error.z);
// set body frame targets for rate controller
_rate_bf_target.y = _rate_stab_bf_target.y + rate_correction;
// calculate body-frame angle error by integrating body-frame rate error
_rate_stab_bf_error.z += (_rate_stab_bf_target.z - (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt;
// limit maximum error
_rate_stab_bf_error.z = constrain_float(_rate_stab_bf_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
// To-Do: handle case of motors being disarmed or g.rc_3.servo_out == 0 and set error to zero
}
//
// rate controller (earth-frame) methods
//
// rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets
void AC_AttitudeControl::rate_ef_targets_to_bf()
{
// convert earth frame rates to body frame rates
_rate_bf_target.x = _rate_ef_target.x - _sin_pitch * _rate_ef_target.z;
_rate_bf_target.y = _cos_roll * _rate_ef_target.y + _sin_roll * _cos_pitch * _rate_ef_target.z;
_rate_bf_target.z = _cos_pitch * _cos_roll * _rate_ef_target.z - _sin_roll * _rate_ef_target.y;
}
//
// rate controller (body-frame) methods
//
// rate_controller_run - run lowest level rate controller and send outputs to the motors
// should be called at 100hz or more
void AC_AttitudeControl::rate_controller_run()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
// To-Do: skip this step if the throttle out is zero?
_motor_roll = rate_bf_to_motor_roll(_rate_bf_target.x);
_motor_pitch = rate_bf_to_motor_pitch(_rate_bf_target.y);
_motor_yaw = rate_bf_to_motor_yaw(_rate_bf_target.z);
// To-Do: what about throttle?
//_motor_
}
//
// private methods
//
//
// body-frame rate controller
//
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
{
float p,i,d; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
// get current rate
// To-Do: make getting gyro rates more efficient?
current_rate = (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100);
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
p = _pid_rate_roll.get_p(rate_error);
// get i term
i = _pid_rate_roll.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = _pid_rate_roll.get_i(rate_error, _dt);
}
// get d term
d = _pid_rate_roll.get_d(rate_error, _dt);
// constrain output and return
return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
// To-Do: allow logging of PIDs?
}
// rate_bf_to_motor_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds)
{
float p,i,d; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
// get current rate
// To-Do: make getting gyro rates more efficient?
current_rate = (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100);
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
p = _pid_rate_pitch.get_p(rate_error);
// get i term
i = _pid_rate_pitch.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = _pid_rate_pitch.get_i(rate_error, _dt);
}
// get d term
d = _pid_rate_pitch.get_d(rate_error, _dt);
// constrain output and return
return constrain_float((p+i+d), -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
// To-Do: allow logging of PIDs?
}
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds)
{
float p,i,d; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
// get current rate
// To-Do: make getting gyro rates more efficient?
current_rate = (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100);
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
p = _pid_rate_yaw.get_p(rate_error);
// separately calculate p, i, d values for logging
p = _pid_rate_yaw.get_p(rate_error);
// get i term
i = _pid_rate_yaw.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_motors.limit.yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = _pid_rate_yaw.get_i(rate_error, _dt);
}
// get d value
d = _pid_rate_yaw.get_d(rate_error, _dt);
// constrain output and return
return constrain_float((p+i+d), -AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);
// To-Do: allow logging of PIDs?
}
//
// throttle functions
//
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost)
{
if( apply_angle_boost ) {
_motor_throttle = get_angle_boost(throttle_out);
}else{
_motor_throttle = throttle_out;
// clear angle_boost for logging purposes
//angle_boost = 0;
}
// update compass with throttle value
// To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class
//compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
}
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm) const
{
float temp = _cos_pitch * _cos_roll;
int16_t throttle_out;
temp = constrain_float(temp, 0.5f, 1.0f);
// reduce throttle if we go inverted
temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
// apply scale and constrain throttle
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
//throttle_out = constrain_float((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
// to allow logging of angle boost
// To-Do: expose this so it can be logged?
//angle_boost = throttle_out - throttle_pwm;
return throttle_out;
}

245
libraries/AC_AttitudeControl/AC_AttitudeControl.h Normal file
View File

@ -0,0 +1,245 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AC_AttitudeControl.h
/// @brief ArduCopter attitude control library
#ifndef AC_AttitudeControl_H
#define AC_AttitudeControl_H
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_InertialSensor.h>
#include <AP_AHRS.h>
#include <AP_Motors.h>
#include <DataFlash.h>
#include <AC_PID.h>
#include <APM_PI.h>
// To-Do: change the name or move to AP_Math?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 5000.0f // body-frame rate controller maximum output (for roll-pitch axis)
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis)
#define AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX 4500.0f // earth-frame angle controller maximum output (for yaw axis)
#define AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX 4500.0f // earth-frame angle controller maximum input angle (To-Do: replace with reference to aparm.angle_max)
#define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX 1000.0f // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate
#define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
class AC_AttitudeControl {
public:
AC_AttitudeControl( AP_AHRS &ahrs,
AP_InertialSensor& ins,
const AP_Vehicle::MultiCopter &parms,
AP_Motors& motors,
APM_PI& pi_angle_roll, APM_PI& pi_angle_pitch, APM_PI& pi_angle_yaw,
AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw,
int16_t& motor_roll, int16_t& motor_pitch, int16_t& motor_yaw, int16_t& motor_throttle
) :
_ahrs(ahrs),
_ins(ins),
aparm(parms),
_motors(motors),
_pi_angle_roll(pi_angle_roll),
_pi_angle_pitch(pi_angle_pitch),
_pi_angle_yaw(pi_angle_yaw),
_pid_rate_roll(pid_rate_roll),
_pid_rate_pitch(pid_rate_pitch),
_pid_rate_yaw(pid_rate_yaw),
_motor_roll(motor_roll),
_motor_pitch(motor_pitch),
_motor_yaw(motor_yaw),
_motor_throttle(motor_throttle),
_dt(AC_ATTITUDE_100HZ_DT)
{
AP_Param::setup_object_defaults(this, var_info);
}
//
// initialisation functions
//
// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void set_dt(float delta_sec) { _dt = delta_sec; }
float get_dt() { return _dt; }
//
// angle controller (earth-frame) methods
//
// angle_ef_targets - get and set angle controller earth-frame-targets
Vector3f angle_ef_targets() const { return _angle_ef_target; }
void angle_ef_targets(Vector3f& angle_cd) { _angle_ef_target = angle_cd; }
// angle_to_rate_ef_roll - converts earth-frame angle targets to earth-frame rate targets for roll, pitch and yaw axis
// targets angles in centi-degrees should be placed in _angle_ef_targets
// results in centi-degrees/sec put directly into _rate_ef_target
void angle_to_rate_ef_roll();
void angle_to_rate_ef_pitch();
void angle_to_rate_ef_yaw();
//
// stabilized rate controller (earth-frame) methods
//
// rate_stab_ef_targets - gets and sets the stabilized rate controller earth-frame targets
// stabilized rate controllers are better at maintaining a desired rate than the simpler earth-frame rate controllers
// because they also maintain angle-targets and increase/decrease the rate request passed to the earth-frame rate controller
// upon the errors between the actual angle and angle-target.
//
Vector3f rate_stab_ef_targets() const { return _rate_stab_ef_target; }
void rate_stab_ef_targets(Vector3f& rates_cds) { _rate_stab_ef_target = rates_cds; }
// rate_stab_ef_to_rate_ef_roll - converts earth-frame stabilized rate targets to regular earth-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees/second taken from _rate_stab_ef_target
// results in centi-degrees/sec put into _rate_ef_target
void rate_stab_ef_to_rate_ef_roll();
void rate_stab_ef_to_rate_ef_pitch();
void rate_stab_ef_to_rate_ef_yaw();
//
// stabilized rate controller (body-frame) methods
//
// rate_stab_bf_targets - gets and sets the stabilized rate controller body-frame targets
Vector3f rate_stab_bf_targets() const { return _rate_stab_bf_target; }
void rate_stab_bf_targets(Vector3f& rates_cds) { _rate_stab_bf_target = rates_cds; }
// rate_stab_bf_to_rate_ef_roll - converts body-frame stabilized rate targets to regular body-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees/second taken from _rate_stab_bf_target
// results in centi-degrees/sec put into _rate_bf_target
void rate_stab_bf_to_rate_bf_roll();
void rate_stab_bf_to_rate_bf_pitch();
void rate_stab_bf_to_rate_bf_yaw();
//
// rate controller (earth-frame) methods
//
// rate_ef_targets - gets and sets rate controller earth-frame-targets
Vector3f rate_ef_targets() const { return _rate_ef_target; }
void rate_ef_targets(Vector3f& rates_cds) { _rate_ef_target = rates_cds; }
// rate_ef_targets_to_bf - converts earth frame rate targets to body frame rate targets
void rate_ef_targets_to_bf();
//
// rate controller (body-frame) methods
//
// rate_bf_targets - gets and sets rate controller body-frame targets
// To-Do: can we return these Vector3fs by reference? i.e. using "&"?
Vector3f rate_bf_targets() const { return _rate_bf_target; }
void rate_bf_targets(Vector3f& rates_cds) { _rate_bf_target = rates_cds; }
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
void rate_controller_run();
//
// throttle functions
//
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void set_throttle_out(int16_t throttle_pwm, bool apply_angle_boost);
//
// helper functions
//
/// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
/// To-Do: make these references or calculate in ahrs
void set_cos_sin_yaw(float cos_roll, float cos_pitch, float sin_roll, float sin_pitch) {
_cos_roll = cos_roll;
_cos_pitch = cos_pitch;
_sin_roll = sin_roll;
_sin_pitch = sin_pitch;
}
// user settable parameters
static const struct AP_Param::GroupInfo var_info[];
private:
//
// body-frame rate controller
//
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in centi-degrees/sec) for roll, pitch and yaw
float rate_bf_to_motor_roll(float rate_target_cds);
float rate_bf_to_motor_pitch(float rate_target_cds);
float rate_bf_to_motor_yaw(float rate_target_cds);
//
// throttle methods
//
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
int16_t get_angle_boost(int16_t throttle_pwm) const;
//
// logging
//
// log data on internal state of the controller. Called at 10Hz
void log_data(DataFlash_Class &dataflash, uint8_t msgid);
// dataflash logging packet
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
int16_t roll_in;
int16_t roll;
int16_t pitch_in;
int16_t pitch;
int16_t yaw_in;
uint16_t yaw;
uint16_t nav_yaw;
} log_ACAttControl;
// references to external libraries
AP_AHRS& _ahrs;
AP_InertialSensor& _ins;
const AP_Vehicle::MultiCopter &aparm;
AP_Motors& _motors;
APM_PI& _pi_angle_roll;
APM_PI& _pi_angle_pitch;
APM_PI& _pi_angle_yaw;
AC_PID& _pid_rate_roll;
AC_PID& _pid_rate_pitch;
AC_PID& _pid_rate_yaw;
int16_t& _motor_roll; // g.rc_1.servo_out
int16_t& _motor_pitch; // g.rc_2.servo_out
int16_t& _motor_yaw; // g.rc_4.servo_out
int16_t& _motor_throttle; // g.rc_3.servo_out
// parameters
AP_Float _dummy_param;
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?
float _dt; // time delta in seconds
Vector3f _angle_ef_target; // angle controller earth-frame targets
Vector3f _rate_stab_ef_target; // stabilized rate controller earth-frame rate targets
Vector3f _rate_ef_target; // rate controller earth-frame targets
Vector3f _rate_stab_bf_target; // stabilized rate controller body-frame rate targets
Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors
Vector3f _rate_bf_target; // rate controller body-frame targets
// precalculated values for efficiency saves
// To-Do: could these be changed to references and passed into the constructor?
float _cos_roll;
float _cos_pitch;
float _sin_roll;
float _sin_pitch;
};
#define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \
"ATT", "cccccCC", "RollIn,Roll,PitchIn,Pitch,YawIn,Yaw,NavYaw" }
#endif //AC_AttitudeControl_H

89
libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde Normal file
View File

@ -0,0 +1,89 @@
/*
* Example of AC_AttitudeControl library
* DIYDrones.com
*/
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_GPS.h> // ArduPilot GPS library
#include <AP_GPS_Glitch.h> // GPS glitch protection library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
#include <AP_Baro.h> // ArduPilot Mega Barometer Library
#include <Filter.h>
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Declination.h>
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AC_PID.h> // PID library
#include <APM_PI.h> // PID library
#include <AP_Buffer.h> // ArduPilot general purpose FIFO buffer
#include <AP_InertialNav.h> // Inertial Navigation library
#include <GCS_MAVLink.h>
#include <AP_Notify.h>
#include <AP_Vehicle.h>
#include <DataFlash.h>
#include <RC_Channel.h> // RC Channel Library
#include <AP_Motors.h>
#include <AC_AttitudeControl.h>
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
// INS and Baro declaration
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
AP_InertialSensor_MPU6000 ins;
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
#else
AP_ADC_ADS7844 adc;
AP_InertialSensor_Oilpan ins(&adc);
AP_Baro_BMP085 baro;
#endif
// GPS declaration
GPS *gps;
AP_GPS_Auto auto_gps(&gps);
GPS_Glitch gps_glitch(gps);
AP_Compass_HMC5843 compass;
AP_AHRS_DCM ahrs(&ins, gps);
// Inertial Nav declaration
AP_InertialNav inertial_nav(&ahrs, &ins, &baro, gps, gps_glitch);
// fake PIDs
APM_PI pi_angle_roll, pi_angle_pitch, pi_angle_yaw;
AC_PID pid_rate_roll, pid_rate_pitch, pid_rate_yaw;
// fake RC inputs
RC_Channel rc_roll(CH_1), rc_pitch(CH_2), rc_yaw(CH_4), rc_throttle(CH_3);
// fake motor and outputs
AP_MotorsQuad motors(&rc_roll, &rc_pitch, &rc_throttle, &rc_yaw);
int16_t motor_roll, motor_pitch, motor_yaw, motor_throttle;
// Attitude Control
AC_AttitudeControl ac_control(ahrs, ins, motors, pi_angle_roll, pi_angle_pitch, pi_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw, motor_roll, motor_pitch, motor_yaw, motor_throttle);
void setup()
{
hal.console->println("AC_AttitudeControl library test");
}
void loop()
{
// call update function
hal.console->printf_P(PSTR("hello"));
hal.scheduler->delay(1);
}
AP_HAL_MAIN();

1
libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/Makefile Normal file
View File

@ -0,0 +1 @@
include ../../../../mk/apm.mk

0
libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/nocore.inoflag Normal file
View File