forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 4
/
SIM_Aircraft.h
339 lines (267 loc) · 9.75 KB
/
SIM_Aircraft.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
parent class for aircraft simulators
*/
#pragma once
#include <AP_Math/AP_Math.h>
#include "SITL.h"
#include "SITL_Input.h"
#include "SIM_Sprayer.h"
#include "SIM_Gripper_Servo.h"
#include "SIM_Gripper_EPM.h"
#include "SIM_Parachute.h"
#include "SIM_Precland.h"
#include "SIM_RichenPower.h"
#include "SIM_I2C.h"
#include "SIM_Buzzer.h"
#include "SIM_Battery.h"
#include <Filter/Filter.h>
#include "SIM_JSON_Master.h"
namespace SITL {
/*
parent class for all simulator types
*/
class Aircraft {
public:
Aircraft(const char *frame_str);
// called directly after constructor:
virtual void set_start_location(const Location &start_loc, const float start_yaw);
/*
set simulation speedup
*/
void set_speedup(float speedup);
float get_speedup() const { return target_speedup; }
/*
set instance number
*/
void set_instance(uint8_t _instance) {
instance = _instance;
}
/*
set directory for additional files such as aircraft models
*/
void set_autotest_dir(const char *_autotest_dir) {
autotest_dir = _autotest_dir;
}
/* Create and set in/out socket for extenal simulator */
virtual void set_interface_ports(const char* address, const int port_in, const int port_out) {};
/*
step the FDM by one time step
*/
virtual void update(const struct sitl_input &input) = 0;
void update_model(const struct sitl_input &input);
/* fill a sitl_fdm structure from the simulator state */
void fill_fdm(struct sitl_fdm &fdm);
/* smooth sensors to provide kinematic consistancy */
void smooth_sensors(void);
/* return normal distribution random numbers */
static double rand_normal(double mean, double stddev);
// get frame rate of model in Hz
float get_rate_hz(void) const { return rate_hz; }
// get number of motors for model
uint16_t get_num_motors() const {
return num_motors;
}
// get motor offset for model
virtual uint16_t get_motors_offset() const {
return 0;
}
const Vector3f &get_gyro(void) const {
return gyro;
}
const Vector3f &get_velocity_ef(void) const {
return velocity_ef;
}
const Vector3f &get_velocity_air_ef(void) const {
return velocity_air_ef;
}
const Matrix3f &get_dcm(void) const {
return dcm;
}
const Vector3f &get_mag_field_bf(void) const {
return mag_bf;
}
float gross_mass() const { return mass + external_payload_mass; }
virtual void set_config(const char* config) {
config_ = config;
}
const Location &get_location() const { return location; }
// get position relative to home
Vector3d get_position_relhome() const;
// distance the rangefinder is perceiving
float rangefinder_range() const;
void get_attitude(Quaternion &attitude) const {
attitude.from_rotation_matrix(dcm);
}
const Location &get_home() const { return home; }
float get_home_yaw() const { return home_yaw; }
void set_buzzer(Buzzer *_buzzer) { buzzer = _buzzer; }
void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
void set_parachute(Parachute *_parachute) { parachute = _parachute; }
void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; }
void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
void set_precland(SIM_Precland *_precland);
void set_i2c(class I2C *_i2c) { i2c = _i2c; }
float get_battery_voltage() const { return battery_voltage; }
protected:
SITL *sitl;
// origin of position vector
Location origin;
// home location
Location home;
bool home_is_set;
Location location;
float ground_level;
float home_yaw;
float frame_height;
Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
Vector3f gyro; // rad/s
Vector3f velocity_ef; // m/s, earth frame
Vector3f wind_ef; // m/s, earth frame
Vector3f velocity_air_ef; // velocity relative to airmass, earth frame
Vector3f velocity_air_bf; // velocity relative to airmass, body frame
Vector3d position; // meters, NED from origin
float mass; // kg
float external_payload_mass; // kg
Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame
float airspeed; // m/s, apparent airspeed
float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube
float battery_voltage = -1.0f;
float battery_current;
float local_ground_level; // ground level at local position
bool lock_step_scheduled;
uint32_t last_one_hz_ms;
// battery model
Battery battery;
uint8_t num_motors = 1;
uint8_t vtol_motor_start;
float rpm[12];
uint8_t rcin_chan_count;
float rcin[12];
float range = -1.0f; // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude
struct {
// data from simulated laser scanner, if available
struct vector3f_array points;
struct float_array ranges;
} scanner;
// Rangefinder
float rangefinder_m[RANGEFINDER_MAX_INSTANCES];
// Windvane apparent wind
struct {
float speed;
float direction;
} wind_vane_apparent;
// Wind Turbulence simulated Data
float turbulence_azimuth;
float turbulence_horizontal_speed; // m/s
float turbulence_vertical_speed; // m/s
Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame
uint64_t time_now_us;
const float gyro_noise = radians(0.1f);
const float accel_noise = 0.3f;
float rate_hz = 1200.0f;
float target_speedup;
uint64_t frame_time_us;
uint64_t last_wall_time_us;
uint32_t last_fps_report_ms;
int64_t sleep_debt_us;
uint32_t last_frame_count;
uint8_t instance;
const char *autotest_dir;
const char *frame;
bool use_time_sync = true;
float last_speedup = -1.0f;
const char *config_ = "";
// allow for AHRS_ORIENTATION
AP_Int8 *ahrs_orientation;
enum Rotation last_imu_rotation;
AP_Float* custom_roll;
AP_Float* custom_pitch;
AP_Float* custom_yaw;
enum GroundBehaviour {
GROUND_BEHAVIOR_NONE = 0,
GROUND_BEHAVIOR_NO_MOVEMENT,
GROUND_BEHAVIOR_FWD_ONLY,
GROUND_BEHAVIOR_TAILSITTER,
} ground_behavior;
bool use_smoothing;
float ground_height_difference() const;
virtual bool on_ground() const;
// returns height above ground level in metres
float hagl() const; // metres
/* update location from position */
void update_position(void);
/* update body frame magnetic field */
void update_mag_field_bf(void);
/* advance time by deltat in seconds */
void time_advance();
/* setup the frame step time */
void setup_frame_time(float rate, float speedup);
/* adjust frame_time calculation */
void adjust_frame_time(float rate);
/* try to synchronise simulation time with wall clock time, taking
into account desired speedup */
void sync_frame_time(void);
/* add noise based on throttle level (from 0..1) */
void add_noise(float throttle);
/* return a monotonic wall clock time in microseconds */
uint64_t get_wall_time_us(void) const;
// update attitude and relative position
void update_dynamics(const Vector3f &rot_accel);
// update wind vector
void update_wind(const struct sitl_input &input);
// return filtered servo input as -1 to 1 range
float filtered_idx(float v, uint8_t idx);
float filtered_servo_angle(const struct sitl_input &input, uint8_t idx);
float filtered_servo_range(const struct sitl_input &input, uint8_t idx);
// extrapolate sensors by a given delta time in seconds
void extrapolate_sensors(float delta_time);
// update external payload/sensor dynamic
void update_external_payload(const struct sitl_input &input);
void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
void add_twist_forces(Vector3f &rot_accel);
// get local thermal updraft
float get_local_updraft(const Vector3d ¤tPos);
private:
uint64_t last_time_us;
uint32_t frame_counter;
uint32_t last_ground_contact_ms;
#if defined(__CYGWIN__) || defined(__CYGWIN64__)
const uint32_t min_sleep_time{20000};
#else
const uint32_t min_sleep_time{5000};
#endif
struct {
Vector3f accel_body;
Vector3f gyro;
Matrix3f rotation_b2e;
Vector3d position;
Vector3f velocity_ef;
uint64_t last_update_us;
Location location;
} smoothing;
LowPassFilterFloat servo_filter[5];
Buzzer *buzzer;
Sprayer *sprayer;
Gripper_Servo *gripper;
Gripper_EPM *gripper_epm;
Parachute *parachute;
RichenPower *richenpower;
IntelligentEnergy24 *ie24;
SIM_Precland *precland;
class I2C *i2c;
};
} // namespace SITL