Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_passthrough.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Freek van Tienen
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
31 #include "state.h"
35 #include "paparazzi.h"
36 
37 #include "generated/airframe.h"
38 
39 #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK)
40 
42 
43 
45 {
47 }
48 
49 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
50 {
51  //Read from RC
52  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
53 }
54 
56 {
57 
58 }
59 
60 void stabilization_attitude_run(bool in_flight __attribute__((unused)))
61 {
62 
63  /* For roll and pitch we pass trough the desired angles as stabilization command */
64  const int32_t angle2cmd = (MAX_PPRZ / TRAJ_MAX_BANK);
65  stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
66  stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
67 
68  //TODO: Fix yaw with PID controller
70  INT32_ANGLE_NORMALIZE(yaw_error);
71  // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
72 
73  /* bound the result */
74  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
75  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
76  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
77 }
78 
80 {
84 }
85 
87 {
88  stab_att_sp_euler = *rpy;
89 }
90 
92 {
93  /* Rotate horizontal commands to body frame by psi */
95  int32_t s_psi, c_psi;
96  PPRZ_ITRIG_SIN(s_psi, psi);
97  PPRZ_ITRIG_COS(c_psi, psi);
98  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
99  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
101 }
int32_t psi
in rad with INT32_ANGLE_FRAC
void stabilization_attitude_set_failsafe_setpoint(void)
void stabilization_attitude_enter(void)
void stabilization_attitude_run(bool in_flight)
General attitude stabilization interface for rotorcrafts.
int32_t theta
in rad with INT32_ANGLE_FRAC
void stabilization_attitude_init(void)
stabilization_attitude_init
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Read an attitude setpoint from the RC.
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
#define PPRZ_ITRIG_SIN(_s, _a)
static float heading
Definition: ahrs_infrared.c:45
euler angles
#define INT32_ANGLE_NORMALIZE(_a)
signed long int32_t
Definition: types.h:19
#define INT32_TRIG_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
API to get/set the generic vehicle states.
General stabilization interface for rotorcrafts.
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as euler angles.
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
#define PPRZ_ITRIG_COS(_c, _a)