Paparazzi UAS  v6.0_unstable-38-g4d1c433-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
logger_control_effectiveness.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
28 #include "mcu_periph/sys_time.h"
29 #include "state.h"
30 
31 // define parameters logged by default
32 
33 #ifndef LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
34 #define LOGGER_CONTROL_EFFECTIVENESS_COMMANDS TRUE
35 #endif
36 
37 #ifndef LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
38 #define LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS FALSE
39 #endif
40 
41 #ifndef LOGGER_CONTROL_EFFECTIVENESS_POS
42 #define LOGGER_CONTROL_EFFECTIVENESS_POS FALSE
43 #endif
44 
45 #ifndef LOGGER_CONTROL_EFFECTIVENESS_SPEED
46 #define LOGGER_CONTROL_EFFECTIVENESS_SPEED FALSE
47 #endif
48 
49 #ifndef LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED
50 #define LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED FALSE
51 #endif
52 
53 // extra includes
54 
55 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
56 #ifdef ROTORCRAFT_FIRMWARE
58 #endif
59 #ifdef FIXEDWING_FIRMWARE
60 #include "subsystems/commands.h"
61 #endif
62 #endif
63 
64 #if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
65 #include "subsystems/actuators.h"
66 #endif
67 
70 {
71  if (pprzLogFile != -1) {
72  sdLogWriteLog(pprzLogFile, "time,gyro_p,gyro_q,gyro_r,ax,ay,az");
73 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
74  for (unsigned int i = 0; i < COMMANDS_NB; i++) {
75  sdLogWriteLog(pprzLogFile, ",cmd_%d", i);
76  }
77 #endif
78 #if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
79  for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
80  sdLogWriteLog(pprzLogFile, ",act_%d", i);
81  }
82 #endif
83 #if LOGGER_CONTROL_EFFECTIVENESS_POS
84  sdLogWriteLog(pprzLogFile, ",pos_x,pos_y,pos_z");
85 #endif
86 #if LOGGER_CONTROL_EFFECTIVENESS_SPEED
87  sdLogWriteLog(pprzLogFile, ",speed_x,speed_y,speed_z");
88 #endif
89 #if LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED
90  sdLogWriteLog(pprzLogFile, ",eas");
91 #endif
92  sdLogWriteLog(pprzLogFile,"\n");
93  }
94 }
95 
96 
99 {
100  if (pprzLogFile == -1) {
101  return;
102  }
103 
104  struct Int32Rates *rates = stateGetBodyRates_i();
105  struct Int32Vect3 *accel_body = stateGetAccelBody_i();
106 
107  // log time, rate and accel
108  sdLogWriteLog(pprzLogFile, "%.5f,%ld,%ld,%ld,%ld,%ld,%ld",
110  rates->p,
111  rates->q,
112  rates->r,
113  accel_body->x,
114  accel_body->y,
115  accel_body->z);
116 
117  // log commands
118 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
119  for (unsigned int i = 0; i < COMMANDS_NB; i++) {
120 #ifdef ROTORCRAFT_FIRMWARE
121  sdLogWriteLog(pprzLogFile, ",%ld", stabilization_cmd[i]);
122 #endif
123 #ifdef FIXEDWING_FIRMWARE
124  sdLogWriteLog(pprzLogFile, ",%d", commands[i]);
125 #endif
126  }
127 #endif
128 
129  // log actuators
130 #if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
131  for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
132  sdLogWriteLog(pprzLogFile, ",%d", actuators[i]);
133  }
134 #endif
135 
136  // log position
137 #if LOGGER_CONTROL_EFFECTIVENESS_POS
138  struct EnuCoor_i *pos = stateGetPositionEnu_i();
139  sdLogWriteLog(pprzLogFile, ",%ld,%ld,%ld", pos->x, pos->y, pos->z);
140 #endif
141 
142  // log speed
143 #if LOGGER_CONTROL_EFFECTIVENESS_SPEED
144  struct EnuCoor_i *speed = stateGetSpeedEnu_i();
145  sdLogWriteLog(pprzLogFile, ",%ld,%ld,%ld", speed->x, speed->y, speed->z);
146 #endif
147 
148  // log airspeed
149 #if LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED
150  sdLogWriteLog(pprzLogFile, ",%.2f", stateGetAirspeed_f());
151 #endif
152 
153  // end line
154  sdLogWriteLog(pprzLogFile,"\n");
155 }
156 
angular rates
void logger_control_effectiveness_periodic(void)
Log the values to file.
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
int32_t y
North.
int32_t x
East.
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
int32_t r
in rad/s with INT32_RATE_FRAC
Hardware independent API for actuators (servos, motor controllers).
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
void logger_control_effectiveness_start(void)
Write the log header line according to the enabled parts.
Architecture independent timing functions.
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
Hardware independent code for commands handling.
int32_t z
Up.
vector in East North Up coordinates
API to get/set the generic vehicle states.
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
int32_t p
in rad/s with INT32_RATE_FRAC
Log data required to compute control effectiveness.
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
static const ShellCommand commands[]
Definition: shell_arch.c:71
int32_t q
in rad/s with INT32_RATE_FRAC