32#include "generated/airframe.h"
33#include "generated/flight_plan.h"
37#ifndef GUIDANCE_PLANE_MAX_BANK
38#define GUIDANCE_PLANE_MAX_BANK RadOfDeg(45.f)
41#ifndef GUIDANCE_PLANE_MAX_PITCH
42#define GUIDANCE_PLANE_MAX_PITCH RadOfDeg(30.f)
45#ifndef GUIDANCE_PLANE_MIN_PITCH
46#define GUIDANCE_PLANE_MIN_PITCH RadOfDeg(-20.f)
49#ifndef GUIDANCE_PLANCE_COURSE_PRE_BANK
50#define GUIDANCE_PLANCE_COURSE_PRE_BANK 1.f
53#ifndef GUIDANCE_PLANE_MAX_CLIMB
54#define GUIDANCE_PLANE_MAX_CLIMB 2.f
57#ifndef GUIDANCE_PLANE_PITCH_OF_VZ
58#define GUIDANCE_PLANE_PITCH_OF_VZ RadOfDeg(5.f)
61#ifndef GUIDANCE_PLANE_PITCH_TRIM
62#define GUIDANCE_PLANE_PITCH_TRIM RadOfDeg(0.f)
65#ifndef GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT
66#define GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT 0.1f
80#define GUIDANCE_PLANE_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
83#define GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR 0.4
121#if PERIODIC_TELEMETRY
134 guidance_plane.roll_cmd = 0.f;
135 guidance_plane.pitch_cmd = 0.f;
137 if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
138 struct FloatEulers e;
139 float_eulers_of_quat(&e, &nav.quat);
140 guidance_plane.roll_cmd = e.phi;
141 guidance_plane.pitch_cmd = e.theta;
144 guidance_plane.roll_cmd = nav.roll;
145 guidance_plane.pitch_cmd = nav.pitch;
154 switch (
nav.setpoint_mode) {
173 static float last_err = 0.f;
176 float d_err = err - last_err;
195 static float last_err = 0.f;
203 float d_err = err - last_err;
223 static float last_err = 0.;
231 float d_err = err - last_err;
256 guidance_plane.throttle_cmd = (int32_t) nav.throttle;
258 if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
259 guidance_plane.altitude_setpoint = nav.nav_altitude;
260 float alt_err = guidance_plane.altitude_setpoint - stateGetPositionEnu_f()->z;
261 guidance_plane.climb_setpoint = guidance_plane.climb_kp * alt_err;
263 guidance_plane.climb_setpoint = nav.climb;
265 guidance_plane.climb_setpoint = 0.f;
if(GpsFixValid() &&e_identification_started)
#define POS_BFP_OF_REAL(_af)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
#define GUIDANCE_PLANE_PITCH_TRIM
#define GUIDANCE_PLANCE_COURSE_PRE_BANK
struct GuidancePlane guidance_plane
Guidance PID structyre.
#define GUIDANCE_PLANE_MAX_PITCH
#define GUIDANCE_PLANE_MAX_BANK
#define GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT
#define GUIDANCE_PLANE_PITCH_OF_VZ
void guidance_plane_enter(void)
#define GUIDANCE_PLANE_MAX_CLIMB
#define GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR
static void guidance_plane_set_pitch(bool in_flight)
void guidance_plane_init(void)
struct StabilizationSetpoint guidance_plane_attitude_from_nav(bool in_flight UNUSED)
run horizontal control loop for position and speed control
static float pitch_sum_err
#define GUIDANCE_PLANE_PITCH_MAX_SUM_ERR
static void guidance_plane_set_throttle(bool in_flight)
struct ThrustSetpoint guidance_plane_thrust_from_nav(bool in_flight)
run vertical control loop for position and speed control
#define GUIDANCE_PLANE_MIN_PITCH
static float throttle_sum_err
Guidance controller for planes in rotorcraft firmware using basic PID controller no airspeed control.
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
struct HorizontalGuidance guidance_h
struct RotorcraftNavigation nav
Rotorcraft navigation functions.
#define NAV_SETPOINT_MODE_SPEED
#define NAV_VERTICAL_MODE_CLIMB
#define NAV_VERTICAL_MODE_MANUAL
#define NAV_SETPOINT_MODE_ACCEL
#define NAV_HORIZONTAL_MODE_GUIDED
#define NAV_HORIZONTAL_MODE_ATTITUDE
#define NAV_HORIZONTAL_MODE_NONE
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
API to get/set the generic vehicle states.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Periodic telemetry system header (includes downlink utility and generated code).