Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
61#endif
62#endif
63
64#if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
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
91#endif
93 }
94}
95
96
99{
100 if (pprzLogFile == -1) {
101 return;
102 }
103
104 struct Int32Rates *rates = stateGetBodyRates_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
122#endif
123#ifdef FIXEDWING_FIRMWARE
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++) {
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
151#endif
152
153 // end line
155}
156
Hardware independent code for commands handling.
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
angular rates
int32_t y
North.
int32_t z
Up.
int32_t x
East.
vector in East North Up coordinates
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition state.h:803
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition state.h:1358
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition state.h:1094
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition state.h:1013
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1590
void logger_control_effectiveness_periodic(void)
Log the values to file.
void logger_control_effectiveness_start(void)
Write the log header line according to the enabled parts.
Log data required to compute control effectiveness.
uint16_t foo
Definition main_demo5.c:58
Hardware independent API for actuators (servos, motor controllers).
FileDes pprzLogFile
static const ShellCommand commands[]
Definition shell_arch.c:78
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:138