Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
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 "estimator.h"
11 #include "messages.h"
13 #include "mcu_periph/uart.h"
14 #include "generated/airframe.h"
15 #include "subsystems/nav.h"
16 // #include "math/pprz_algebra_int.h"
17 // #include "math/pprz_algebra_float.h"
18 
19 // For Downlink
20 #ifndef DOWNLINK_DEVICE
21 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
22 #endif
23 
24 
25 
32 bool_t benchm_reset;
33 bool_t benchm_go;
34 
35 
36 //uint8_t numOfCount;
37 
38 
39 
40 void flight_benchmark_init( void ) {
44  ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED;
45  ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE;
46  TolerancePosition = BENCHMARK_TOLERANCE_POSITION;
47  benchm_reset = 0;
48  benchm_go = 0;
49 }
50 
52  float Err_airspeed = 0;
53  float Err_altitude = 0;
54  float Err_position = 0;
55 
56  if (benchm_reset){
58  benchm_reset = 0;
59  }
60 
61  if (benchm_go){
62  #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
63  Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint);
64  if (Err_airspeed>ToleranceAispeed){
65  Err_airspeed = Err_airspeed-ToleranceAispeed;
66  SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
67  }
68  #endif
69 
70  #ifdef BENCHMARK_ALTITUDE
71  Err_altitude = fabs(estimator_z - v_ctl_altitude_setpoint);
72  if (Err_altitude>ToleranceAltitude){
73  Err_altitude = Err_altitude-ToleranceAltitude;
74  SquareSumErr_altitude += (Err_altitude * Err_altitude);
75  }
76  #endif
77 
78  #ifdef BENCHMARK_POSITION
79 
80  //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) -----------------
81 
82  // err_temp = waypoints[target].x - estimator_x;
83  float deltaPlaneX = 0;
84  float deltaPlaneY = 0;
85  float Err_position_segment = 0;
86  float Err_position_circle = 0;
87 
88 // if (nav_in_segment){
89  float deltaX = nav_segment_x_2 - nav_segment_x_1;
90  float deltaY = nav_segment_y_2 - nav_segment_y_1;
91  float anglePath = atan2(deltaX,deltaY);
92  deltaPlaneX = nav_segment_x_2 - estimator_x;
93  deltaPlaneY = nav_segment_y_2 - estimator_y;
94  float anglePlane = atan2(deltaPlaneX,deltaPlaneY);
95  float angleDiff = fabs(anglePlane - anglePath);
96  Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY));
97 // }
98 
99 // if (nav_in_circle){
100  deltaPlaneX = nav_circle_x - estimator_x;
101  deltaPlaneY = nav_circle_y - estimator_y;
102  Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius);
103 // }
104  if (Err_position_circle < Err_position_segment){
105  Err_position = Err_position_circle;
106  }
107  else
108  Err_position = Err_position_segment;
109 
110  if (Err_position>TolerancePosition){
111  SquareSumErr_position += (Err_position * Err_position);
112  }
113  #endif
114  }
115 
116  DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position)
117 
118 }
119 
124 }
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 
138 
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:91
bool_t benchm_go
float estimator_y
north position in meters
Definition: estimator.c:43
float ToleranceAispeed
void flight_benchmark_periodic(void)
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
Vertical control for fixed wing vehicles.
void flight_benchmark_init(void)
float estimator_airspeed
m/s
Definition: estimator.c:69
void flight_benchmark_reset(void)
float SquareSumErr_altitude
float estimator_x
east position in meters
Definition: estimator.c:42
float SquareSumErr_position
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float SquareSumErr_airspeed
State estimation, fusioning sensors.
float ToleranceAltitude
bool_t benchm_reset
float TolerancePosition