ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6D...

94 lines
4.2 KiB
C
Raw Normal View History

2021-01-26 08:54:13 -04:00
#pragma once
#ifdef ENABLE_SCRIPTING
#include "AC_AttitudeControl_Multi.h"
class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
public:
AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt):
AC_AttitudeControl_Multi(ahrs,aparm,motors,dt) {
if (_singleton != nullptr) {
AP_HAL::panic("Can only be one AC_AttitudeControl_Multi_6DoF");
}
_singleton = this;
}
static AC_AttitudeControl_Multi_6DoF *get_singleton() {
return _singleton;
}
// Command a Quaternion attitude with feedforward and smoothing
// not used anywhere in current code, panic so this implementaiton is not overlooked
void input_quaternion(Quaternion attitude_desired_quat) override;
2021-01-26 08:54:13 -04:00
/*
override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
*/
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) override;
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
/*
all other input functions should zero thrust vectoring and behave as a normal copter
*/
// Command euler yaw rate and pitch angle with roll angle specified in body frame
// (used only by tailsitter quadplanes)
void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) override;
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds) override;
// Command an angular velocity with angular velocity feedforward and smoothing
void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
// Command an angular velocity with angular velocity feedforward and smoothing
void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
// Command an angular step (i.e change) in body frame angle
void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd) override;
// run lowest level body-frame rate controller and send outputs to the motors
void rate_controller_run() override;
// limiting lean angle based on throttle makes no sense for 6DoF, always allow 90 deg, return in centi-degrees
float get_althold_lean_angle_max() const override { return 9000.0f; }
// set the attitude that will be used in 6DoF flight
void set_offset_roll_pitch(float roll_deg, float pitch_deg) {
roll_offset_deg = roll_deg;
pitch_offset_deg = pitch_deg;
}
// these flags enable or disable lateral or forward thrust, with both disabled the vehicle acts like a traditional copter
// it will roll and pitch to move, its also possible to enable only forward or lateral to suit the vehicle configuration.
// by default the vehicle is full 6DoF, these can be set in flight via scripting
void set_forward_enable(bool b) {
forward_enable = b;
}
void set_lateral_enable(bool b) {
lateral_enable = b;
}
private:
void set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd);
float roll_offset_deg;
float pitch_offset_deg;
bool forward_enable = true;
bool lateral_enable = true;
static AC_AttitudeControl_Multi_6DoF *_singleton;
};
#endif // ENABLE_SCRIPTING