Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 
33 #include "paparazzi.h"
34 
35 #include "generated/airframe.h"
36 
37 #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK)
38 
40 
41 
44 }
45 
46 
47 void stabilization_attitude_read_rc(bool_t in_flight) {
48  //Read from RC
50 }
51 
52 
54 
55 }
56 
57 void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) {
58 
59  /* For roll and pitch we pass trough the desired angles as stabilization command */
60  const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK);
61  stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
62  stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
63 
64  //TODO: Fix yaw with PID controller
66  INT32_ANGLE_NORMALIZE(yaw_error);
67  // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
68 
69  /* bound the result */
70  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
71  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
72  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
73 }
74 
79 }
80 
82  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
83 }
84 
void stabilization_attitude_read_rc(bool_t in_flight)
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.
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler)
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
Read attitude setpoint from RC as euler angles.
void stabilization_attitude_run(bool_t in_flight)
#define INT32_ANGLE_NORMALIZE(_a)
signed long int32_t
Definition: types.h:19
int32_t psi
in rad with INT32_ANGLE_FRAC
General stabilization interface for rotorcrafts.
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
euler angles