Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
flight_benchmark.c
Go to the documentation of this file.
1//Author: Bruzzlee
2
3//This module allows a quantitative assessment of the flight.
4//It calculates the sum of squared error of the two-dimensional course (x / y), the altitude and true airspeed.
5//The sum of squared error of the course and altitude were separated, because they are regulated separately, and so they dependent on various parameters.
6//The module was written to optimize the control parameters and has already been used successfully.
7
8
10#include "state.h"
11#include "pprzlink/messages.h"
13#include "mcu_periph/uart.h"
14#include "generated/airframe.h"
16// #include "math/pprz_algebra_int.h"
17// #include "math/pprz_algebra_float.h"
18
19// For Downlink
20
29
30//uint8_t numOfCount;
31
33{
37 ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED;
38 ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE;
39 TolerancePosition = BENCHMARK_TOLERANCE_POSITION;
40 benchm_reset = 0;
41 benchm_go = 0;
42}
43
45{
46 float Err_airspeed = 0;
47 float Err_altitude = 0;
48 float Err_position = 0;
49
50 if (benchm_reset) {
52 benchm_reset = 0;
53 }
54
55 if (benchm_go) {
56#if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
57 Err_airspeed = fabs(stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint);
58 if (Err_airspeed > ToleranceAispeed) {
59 Err_airspeed = Err_airspeed - ToleranceAispeed;
60 SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
61 }
62#endif
63
64#ifdef BENCHMARK_ALTITUDE
65 Err_altitude = fabs(stateGetPositionUtm_f()->alt - v_ctl_altitude_setpoint);
66 if (Err_altitude > ToleranceAltitude) {
67 Err_altitude = Err_altitude - ToleranceAltitude;
68 SquareSumErr_altitude += (Err_altitude * Err_altitude);
69 }
70#endif
71
72#ifdef BENCHMARK_POSITION
73
74 //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) -----------------
75
76 // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x;
77 float deltaPlaneX = 0;
78 float deltaPlaneY = 0;
79 float Err_position_segment = 0;
80 float Err_position_circle = 0;
81
82// if (nav_in_segment){
83 float deltaX = nav_segment_x_2 - nav_segment_x_1;
84 float deltaY = nav_segment_y_2 - nav_segment_y_1;
85 float anglePath = atan2(deltaX, deltaY);
86 deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x;
87 deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y;
88 float anglePlane = atan2(deltaPlaneX, deltaPlaneY);
89 float angleDiff = fabs(anglePlane - anglePath);
90 Err_position_segment = fabs(sin(angleDiff) * sqrt(deltaPlaneX * deltaPlaneX + deltaPlaneY * deltaPlaneY));
91// }
92
93// if (nav_in_circle){
94 deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x;
95 deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y;
96 Err_position_circle = fabs(sqrt(deltaPlaneX * deltaPlaneX + deltaPlaneY * deltaPlaneY) - nav_circle_radius);
97// }
98 if (Err_position_circle < Err_position_segment) {
99 Err_position = Err_position_circle;
100 } else {
101 Err_position = Err_position_segment;
102 }
103
104 if (Err_position > TolerancePosition) {
105 SquareSumErr_position += (Err_position * Err_position);
106 }
107#endif
108 }
109
111 &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position);
112
113}
114
121
122
123
124
125
126
127
128
129
130
131
132
133
134
float v_ctl_altitude_setpoint
in meters above MSL
Definition energy_ctrl.c:88
float v_ctl_auto_airspeed_setpoint
in meters per second
Vertical control for fixed wing vehicles.
float TolerancePosition
void flight_benchmark_periodic(void)
void flight_benchmark_init(void)
float SquareSumErr_position
bool benchm_go
float SquareSumErr_airspeed
float ToleranceAispeed
float ToleranceAltitude
void flight_benchmark_reset(void)
float SquareSumErr_altitude
bool benchm_reset
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
float nav_segment_x_2
Definition nav.c:69
float nav_segment_y_2
Definition nav.c:69
float nav_circle_radius
Definition nav.c:68
float nav_circle_y
Definition nav.c:68
float nav_segment_x_1
Definition nav.c:69
float nav_segment_y_1
Definition nav.c:69
float nav_circle_x
Definition nav.c:68
Fixedwing Navigation library.
float y
in meters
float x
in meters
API to get/set the generic vehicle states.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API