Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
28 bool benchm_go;
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 
110  DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude,
111  &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position);
112 
113 }
114 
116 {
120 }
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
Definition: energy_ctrl.c:121
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 EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
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