41 #define CMD_OF_SAT 1500
44 #ifndef VISION_PHI_PGAIN
45 #define VISION_PHI_PGAIN 400
49 #ifndef VISION_PHI_IGAIN
50 #define VISION_PHI_IGAIN 20
54 #ifndef VISION_THETA_PGAIN
55 #define VISION_THETA_PGAIN 400
59 #ifndef VISION_THETA_IGAIN
60 #define VISION_THETA_IGAIN 20
64 #ifndef VISION_DESIRED_VX
65 #define VISION_DESIRED_VX 0
69 #ifndef VISION_DESIRED_VY
70 #define VISION_DESIRED_VY 0
75 #if (VISION_PHI_PGAIN < 0) || \
76 (VISION_PHI_IGAIN < 0) || \
77 (VISION_THETA_PGAIN < 0) || \
78 (VISION_THETA_IGAIN < 0)
79 #error "ALL control gains have to be positive!!!"
205 float s_psi = sin(psi);
206 float c_psi = cos(psi);
214 v_x = speed_cur.
y * 100;
215 v_y = speed_cur.
x * 100;
311 float v_desired_total;
Core autopilot interface common to all firmwares.
Common code for AP and FBW telemetry.
Interface for electrical status: supply voltage, current, battery status, etc.
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_ANGLE_NORMALIZE(_a)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
struct NedCoor_f opti_speed_read
struct opticflow_stab_t opticflow_stab
struct FloatVect3 Total_force
void guidance_h_module_read_rc(void)
Read the RC commands.
#define VISION_THETA_PGAIN
#define VISION_DESIRED_VY
void guidance_h_module_run(bool in_flight)
Main guidance loop.
int8_t repulsionforce_filter_flag
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
#define VISION_DESIRED_VX
#define VISION_THETA_IGAIN
void OA_update()
Update the controls based on a vision result.
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Guidance for the obstacle avoidance methods.
float err_vx_int
The integrated velocity error in x direction (m/s)
struct Int32Eulers cmd
The commands that are send to the hover loop.
float err_vy_int
The integrated velocity error in y direction (m/s)
int32_t theta_pgain
The pitch P gain on the err_vy.
int32_t theta_igain
The pitch I gain on the err_vy_int.
float desired_vy
The desired velocity in the y direction (cm/s)
float desired_vx
The desired velocity in the x direction (cm/s)
int32_t phi_pgain
The roll P gain on the err_vx.
int32_t phi_igain
The roll I gain on the err_vx_int.
vector in North East Down coordinates Units: meters
Vertical guidance for rotorcrafts.
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
void stabilization_attitude_run(bool in_flight)
Periodic telemetry system header (includes downlink utility and generated code).
int int32_t
Typedef defining 32 bit int type.
signed char int8_t
Typedef defining 8 bit char type.