Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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"
34 #include "paparazzi.h"
35 
36 #include "generated/airframe.h"
37 
38 #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK)
39 
41 
42 
45 }
46 
47 void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
48  //Read from RC
49  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
50 }
51 
53 
54 }
55 
56 void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) {
57 
58  /* For roll and pitch we pass trough the desired angles as stabilization command */
59  const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK);
60  stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
61  stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
62 
63  //TODO: Fix yaw with PID controller
65  INT32_ANGLE_NORMALIZE(yaw_error);
66  // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
67 
68  /* bound the result */
69  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
70  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
71  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
72 }
73 
78 }
79 
81  memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers));
82 }
83 
85  /* Rotate horizontal commands to body frame by psi */
87  int32_t s_psi, c_psi;
88  PPRZ_ITRIG_SIN(s_psi, psi);
89  PPRZ_ITRIG_COS(c_psi, psi);
90  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
91  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
93 }
int32_t phi
in rad with INT32_ANGLE_FRAC
void stabilization_attitude_set_failsafe_setpoint(void)
void stabilization_attitude_enter(void)
void stabilization_attitude_init(void)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Read an attitude setpoint from the RC.
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as euler angles.
#define INT32_TRIG_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
float heading
Definition: ahrs_infrared.c:40
void stabilization_attitude_run(bool_t in_flight)
#define INT32_ANGLE_NORMALIZE(_a)
signed long int32_t
Definition: types.h:19
API to get/set the generic vehicle states.
int32_t psi
in rad with INT32_ANGLE_FRAC
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:28
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
euler angles