Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 "state.h"
11 #include "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 
21 
22 
29 bool_t benchm_reset;
30 bool_t benchm_go;
31 
32 
33 //uint8_t numOfCount;
34 
35 
36 
37 void flight_benchmark_init( void ) {
41  ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED;
42  ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE;
43  TolerancePosition = BENCHMARK_TOLERANCE_POSITION;
44  benchm_reset = 0;
45  benchm_go = 0;
46 }
47 
49  float Err_airspeed = 0;
50  float Err_altitude = 0;
51  float Err_position = 0;
52 
53  if (benchm_reset){
55  benchm_reset = 0;
56  }
57 
58  if (benchm_go){
59  #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
60  Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint);
61  if (Err_airspeed>ToleranceAispeed){
62  Err_airspeed = Err_airspeed-ToleranceAispeed;
63  SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
64  }
65  #endif
66 
67  #ifdef BENCHMARK_ALTITUDE
68  Err_altitude = fabs(stateGetPositionUtm_f()->alt - v_ctl_altitude_setpoint);
69  if (Err_altitude>ToleranceAltitude){
70  Err_altitude = Err_altitude-ToleranceAltitude;
71  SquareSumErr_altitude += (Err_altitude * Err_altitude);
72  }
73  #endif
74 
75  #ifdef BENCHMARK_POSITION
76 
77  //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) -----------------
78 
79  // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x;
80  float deltaPlaneX = 0;
81  float deltaPlaneY = 0;
82  float Err_position_segment = 0;
83  float Err_position_circle = 0;
84 
85 // if (nav_in_segment){
86  float deltaX = nav_segment_x_2 - nav_segment_x_1;
87  float deltaY = nav_segment_y_2 - nav_segment_y_1;
88  float anglePath = atan2(deltaX,deltaY);
89  deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x;
90  deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y;
91  float anglePlane = atan2(deltaPlaneX,deltaPlaneY);
92  float angleDiff = fabs(anglePlane - anglePath);
93  Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY));
94 // }
95 
96 // if (nav_in_circle){
97  deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x;
98  deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y;
99  Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius);
100 // }
101  if (Err_position_circle < Err_position_segment){
102  Err_position = Err_position_circle;
103  }
104  else
105  Err_position = Err_position_segment;
106 
107  if (Err_position>TolerancePosition){
108  SquareSumErr_position += (Err_position * Err_position);
109  }
110  #endif
111  }
112 
113  DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position)
114 
115 }
116 
121 }
122 
123 
124 
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
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 ToleranceAispeed
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
void flight_benchmark_periodic(void)
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
Vertical control for fixed wing vehicles.
void flight_benchmark_init(void)
void flight_benchmark_reset(void)
float SquareSumErr_altitude
float x
in meters
float SquareSumErr_position
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
API to get/set the generic vehicle states.
float SquareSumErr_airspeed
float ToleranceAltitude
bool_t benchm_reset
float TolerancePosition