dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
simsensors.c
Go to the documentation of this file.
1 
17 /*
18  * This program is free software; you can redistribute it and/or modify
19  * it under the terms of the GNU General Public License as published by
20  * the Free Software Foundation; either version 3 of the License, or
21  * (at your option) any later version.
22  *
23  * This program is distributed in the hope that it will be useful, but
24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
26  * for more details.
27  *
28  * You should have received a copy of the GNU General Public License along
29  * with this program; if not, see <http://www.gnu.org/licenses/>
30  *
31  * Additional note on redistribution: The copyright and license notices above
32  * must be maintained in each individual source file that is a derivative work
33  * of this source file; otherwise redistribution is prohibited.
34  */
35 
36 #include <unistd.h>
37 #include <assert.h>
38 
39 #include "pios.h"
40 #include "openpilot.h"
41 #include "physical_constants.h"
42 #include "pios_thread.h"
43 
44 #ifdef PIOS_INCLUDE_SIMSENSORS
45 
46 #include "accels.h"
47 #include "actuatordesired.h"
48 #include "airspeedactual.h"
49 #include "attitudeactual.h"
50 #include "attitudesimulated.h"
51 #include "attitudesettings.h"
52 #include "baroairspeed.h"
53 #include "baroaltitude.h"
54 #include "gyros.h"
55 #include "gyrosbias.h"
56 #include "flightstatus.h"
57 #include "gpsposition.h"
58 #include "gpsvelocity.h"
59 #include "homelocation.h"
60 #include "magnetometer.h"
61 #include "magbias.h"
62 #include "ratedesired.h"
63 #include "systemsettings.h"
64 
65 #include "coordinate_conversions.h"
66 
67 // Private constants
68 #define STACK_SIZE_BYTES 1540
69 #define TASK_PRIORITY PIOS_THREAD_PRIO_HIGH
70 #define SENSOR_PERIOD 2
71 
72 // Private types
73 
74 // Private variables
75 static float accel_bias[3];
76 
77 static float rand_gauss();
78 
79 static int sens_rate = 500;
80 
81 enum sensor_sim_type {MODEL_YASIM, MODEL_QUADCOPTER, MODEL_AIRPLANE, MODEL_CAR} sensor_sim_type;
82 
83 static bool have_gyro_data, have_accel_data, have_mag_data, have_baro_data;
84 
85 static struct pios_sensor_gyro_data gyro_data;
86 static struct pios_sensor_accel_data accel_data;
87 static struct pios_sensor_mag_data mag_data;
88 static struct pios_sensor_baro_data baro_data;
89 
90 #ifdef PIOS_INCLUDE_SIMSENSORS_YASIM
91 extern bool use_yasim;
92 static void simulateYasim();
93 #endif
94 
95 // Private functions
96 static void simsensors_step();
97 static void simulateModelQuadcopter();
98 static void simulateModelAirplane();
99 static void simulateModelCar();
100 
101 static bool simsensors_callback_gyro(void *ctx, void *output,
102  int ms_to_wait, int *next_call)
103 {
104  *next_call = 1000 / sens_rate;
105 
106  simsensors_step();
107 
108  if (have_gyro_data) {
109  have_gyro_data = false;
110 
111  memcpy(output, &gyro_data, sizeof(gyro_data));
112 
113  return true;
114  }
115 
116  return false;
117 }
118 
119 static bool simsensors_callback_accel(void *ctx, void *output,
120  int ms_to_wait, int *next_call)
121 {
122  *next_call = 0;
123 
124  if (have_accel_data) {
125  have_accel_data = false;
126 
127  memcpy(output, &accel_data, sizeof(accel_data));
128 
129  return true;
130  }
131 
132  return false;
133 }
134 
135 static bool simsensors_callback_mag(void *ctx, void *output,
136  int ms_to_wait, int *next_call)
137 {
138  *next_call = 0;
139 
140  if (have_mag_data) {
141  have_mag_data = false;
142 
143  memcpy(output, &mag_data, sizeof(mag_data));
144 
145  return true;
146  }
147 
148  return false;
149 }
150 
151 static bool simsensors_callback_baro(void *ctx, void *output,
152  int ms_to_wait, int *next_call)
153 {
154  *next_call = 0;
155 
156  if (have_baro_data) {
157  have_baro_data = false;
158 
159  memcpy(output, &baro_data, sizeof(baro_data));
160 
161  return true;
162  }
163 
164  return false;
165 }
166 
171 int32_t simsensors_init(void)
172 {
173  if (PIOS_SENSORS_IsRegistered(PIOS_SENSOR_GYRO)) {
174  printf("SimSensorsInitialize: Declining to do anything, real sensors!\n");
175  return -1;
176  }
177 
178  printf("SimSensorsInitialize: Using simulated sensors.\n");
179 
180  PIOS_SENSORS_RegisterCallback(PIOS_SENSOR_GYRO,
181  simsensors_callback_gyro, NULL);
182  PIOS_SENSORS_RegisterCallback(PIOS_SENSOR_ACCEL,
183  simsensors_callback_accel, NULL);
184  PIOS_SENSORS_RegisterCallback(PIOS_SENSOR_MAG,
185  simsensors_callback_mag, NULL);
186  PIOS_SENSORS_RegisterCallback(PIOS_SENSOR_BARO,
187  simsensors_callback_baro, NULL);
188 
189 #ifdef PIOS_INCLUDE_SIMSENSORS_YASIM
190  if (use_yasim) {
191  sens_rate = 200;
192  }
193 #endif
194 
195  PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_ACCEL, sens_rate);
196  PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_GYRO, sens_rate);
197  PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_MAG, 75);
198  PIOS_SENSORS_SetSampleRate(PIOS_SENSOR_BARO, 75);
199 
200  accel_bias[0] = rand_gauss() / 10;
201  accel_bias[1] = rand_gauss() / 10;
202  accel_bias[2] = rand_gauss() / 10;
203 
204  AttitudeSimulatedInitialize();
205  BaroAltitudeInitialize();
206  /* TODO: Airspeed data should properly go through airspeed module.
207  */
208  BaroAirspeedInitialize();
209  GPSPositionInitialize();
210  GPSVelocityInitialize();
211 
212  AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
213 
214  PIOS_SENSORS_SetMaxGyro(1000);
215 
216  printf("SimSensorsInitialize: starting SIMULATED sensors\n");
217 
218  return 0;
219 }
220 
224 int sensors_count;
225 static void simsensors_step()
226 {
227  SystemSettingsData systemSettings;
228  SystemSettingsGet(&systemSettings);
229 
230  switch(systemSettings.AirframeType) {
231  case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
232  case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
233  case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
234  sensor_sim_type = MODEL_AIRPLANE;
235  break;
236  case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
237  case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
238  case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
239  case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
240  case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
241  default:
242  sensor_sim_type = MODEL_QUADCOPTER;
243  break;
244  case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
245  sensor_sim_type = MODEL_CAR;
246  break;
247  }
248 
249 #ifdef PIOS_INCLUDE_SIMSENSORS_YASIM
250  if (use_yasim) {
251  sensor_sim_type = MODEL_YASIM;
252  }
253 #endif
254 
255  sensors_count++;
256 
257  switch(sensor_sim_type) {
258  case MODEL_QUADCOPTER:
259  default:
260  simulateModelQuadcopter();
261  break;
262  case MODEL_AIRPLANE:
263  simulateModelAirplane();
264  break;
265  case MODEL_CAR:
266  simulateModelCar();
267  break;
268 #ifdef PIOS_INCLUDE_SIMSENSORS_YASIM
269  case MODEL_YASIM:
270  simulateYasim();
271  break;
272 #endif
273  }
274 }
275 
276 static void simsensors_scale_controls(float *rpy, float *thrust_out,
277  float max_thrust)
278 {
279  const float ACTUATOR_ALPHA = 0.81f;
280 
281  FlightStatusData flightStatus;
282  FlightStatusGet(&flightStatus);
283  ActuatorDesiredData actuatorDesired;
284  ActuatorDesiredGet(&actuatorDesired);
285 
286  float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Thrust * max_thrust : 0;
287 
288  if (thrust != thrust)
289  thrust = 0;
290 
291  *thrust_out = thrust;
292 
293  float control_scaling = 3000.0f;
294 
295  if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
296  control_scaling = 0;
297  thrust = 0;
298  }
299 
300  // In deg/s
301  rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
302  rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
303  rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
304 }
305 
306 static void simsensors_gyro_set(float *rpy, float noise_scale,
307  float temperature)
308 {
309  assert(isfinite(rpy[0]));
310  assert(isfinite(rpy[1]));
311  assert(isfinite(rpy[2]));
312 
313  gyro_data = (struct pios_sensor_gyro_data) {
314  .x = rpy[0] + rand_gauss() * noise_scale +
315  (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11,
316  .y = rpy[1] + rand_gauss() * noise_scale +
317  (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11,
318  .z = rpy[2] + rand_gauss() * noise_scale +
319  (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11,
320  .temperature = temperature,
321  };
322 
323  have_gyro_data = true;
324 }
325 
326 static void simsensors_accels_set(float *xyz, float *accel_bias,
327  float temperature)
328 {
329  accel_data = (struct pios_sensor_accel_data) {
330  .x = xyz[0] + accel_bias[0],
331  .y = xyz[1] + accel_bias[1],
332  .z = xyz[2] + accel_bias[2],
333  .temperature = temperature,
334  };
335 
336  assert(isfinite(accel_data.x));
337  assert(isfinite(accel_data.y));
338  assert(isfinite(accel_data.z));
339 
340  have_accel_data = true;
341 }
342 
343 static void simsensors_accels_setfromned(double *ned_accel,
344  float (*Rbe)[3][3], float *accel_bias, float temperature)
345 {
346  float xyz[3];
347 
348  xyz[0] = ned_accel[0] * (*Rbe)[0][0] + ned_accel[1] * (*Rbe)[0][1] + ned_accel[2] * (*Rbe)[0][2];
349  xyz[1] = ned_accel[0] * (*Rbe)[1][0] + ned_accel[1] * (*Rbe)[1][1] + ned_accel[2] * (*Rbe)[1][2];
350  xyz[2] = ned_accel[0] * (*Rbe)[2][0] + ned_accel[1] * (*Rbe)[2][1] + ned_accel[2] * (*Rbe)[2][2];
351 
352  simsensors_accels_set(xyz, accel_bias, temperature);
353 }
354 
355 static void simsensors_quat_timestep(float *q, float *qdot) {
356  assert(isfinite(qdot[0]));
357  assert(isfinite(qdot[1]));
358  assert(isfinite(qdot[2]));
359  assert(isfinite(qdot[3]));
360 
361  // Take a time step
362  q[0] = q[0] + qdot[0];
363  q[1] = q[1] + qdot[1];
364  q[2] = q[2] + qdot[2];
365  q[3] = q[3] + qdot[3];
366 
367  float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
368 
369  if (qmag < 0.001f) {
370  q[0] = 1;
371  q[1] = 0;
372  q[2] = 0;
373  q[3] = 0;
374  } else {
375  q[0] = q[0] / qmag;
376  q[1] = q[1] / qmag;
377  q[2] = q[2] / qmag;
378  q[3] = q[3] / qmag;
379  }
380 
381  assert(isfinite(q[0]));
382  assert(isfinite(q[1]));
383  assert(isfinite(q[2]));
384  assert(isfinite(q[3]));
385 }
386 
387 static void simsensors_baro_set(float down, float baro_offset) {
388  baro_data = (struct pios_sensor_baro_data) {
389  .temperature = 33,
390  .pressure = 123, /* XXX */
391  .altitude = -down + baro_offset,
392  };
393 
394  have_baro_data = true;
395 }
396 
397 static void simsensors_baro_drift(float *baro_offset)
398 {
399  if (*baro_offset == 0) {
400  // Hacky initialization
401  *baro_offset = 50;
402  } else {
403  // Very small drift process
404  *baro_offset += rand_gauss() / 100;
405  }
406 }
407 
408 static void simsensors_mag_set(float *be, float (*Rbe)[3][3])
409 {
410  mag_data = (struct pios_sensor_mag_data) {
411  .x = 100 +
412  be[0] * (*Rbe)[0][0] +
413  be[1] * (*Rbe)[0][1] +
414  be[2] * (*Rbe)[0][2],
415  .y = 100 +
416  be[0] * (*Rbe)[1][0] +
417  be[1] * (*Rbe)[1][1] +
418  be[2] * (*Rbe)[1][2],
419  .z = 100 +
420  be[0] * (*Rbe)[2][0] +
421  be[1] * (*Rbe)[2][1] +
422  be[2] * (*Rbe)[2][2],
423  };
424 
425  have_mag_data = true;
426 }
427 
428 #ifdef PIOS_INCLUDE_SIMSENSORS_YASIM
429 static void simulateYasim()
430 {
431  const float GYRO_NOISE_SCALE = 1.0f;
432  const float MAG_PERIOD = 1.0 / 75.0;
433  const float BARO_PERIOD = 1.0 / 20.0;
434  const float GPS_PERIOD = 1.0 / 10.0;
435 
436  struct command {
437  uint32_t magic;
438  uint32_t flags;
439 
440  float roll, pitch, yaw, throttle;
441 
442  float reserved[8];
443 
444  bool armed;
445  } cmd;
446 
447  struct status {
448  uint32_t magic;
449 
450  uint32_t flags;
451 
452  double lat, lon, alt;
453 
454  float p, q, r;
455  float acc[3];
456  float vel[3];
457 
458  /* Provided only to "check" attitude solution */
459  float roll, pitch, hdg;
460 
461  float reserved[4];
462  } status;
463 
464  static bool inited = false;
465 
466  static int rfd, wfd;
467 
468  static float baro_offset;
469 
470  if (!inited) {
471  fprintf(stderr, "Initing external yasim instance\n");
472  inited = true;
473  /* pipefd(0) is the read side of pipe */
474 
475  int child_in, child_out;
476 
477  int pipe_tmp[2];
478 
479  int ret = pipe(pipe_tmp);
480 
481  if (ret < 0) {
482  perror("pipe");
483  exit(1);
484  }
485 
486  child_in = pipe_tmp[0];
487  wfd = pipe_tmp[1];
488 
489  ret = pipe(pipe_tmp);
490 
491  if (ret < 0) {
492  perror("pipe");
493  exit(1);
494  }
495 
496  rfd = pipe_tmp[0];
497  child_out = pipe_tmp[1];
498 
499  ret = fork();
500 
501  if (ret < 0) {
502  perror("fork");
503  exit(1);
504  }
505 
506  if (ret == 0) {
507  /* We are the child */
508  if (dup2(child_in, STDIN_FILENO) < 0) {
509  perror("dup2STDIN");
510  exit(1);
511  }
512 
513  if (dup2(child_out, STDOUT_FILENO) < 0) {
514  perror("dup2STDIN");
515  exit(1);
516  }
517 
518  for (int i = 3; i < 1024; i++) {
519  close(i);
520  }
521 
522  // TODO: Fixup model selection to something nice.
523  execlp("yasim-svr", "yasim-svr", "pa22-160-yasim.xml",
524  NULL);
525 
526  perror("execlp");
527  exit(1);
528  }
529 
530  /* We are the parent. */
531  close(child_in);
532  close(child_out);
533  }
534 
535  int ret = read(rfd, &status, sizeof(status));
536 
537  if (ret != sizeof(status)) {
538  fprintf(stderr, "failed: read-from-yasim %d\n", ret);
539  exit(1);
540  }
541 
542  if (status.magic != 0x00700799) {
543  fprintf(stderr, "Bad magic on read from yasim\n");
544  exit(1);
545  }
546 
547  memset(&cmd, 0, sizeof(cmd));
548 
549  cmd.magic = 0xb33fbeef;
550 
551  FlightStatusData flightStatus;
552  FlightStatusGet(&flightStatus);
553  ActuatorDesiredData actuatorDesired;
554  ActuatorDesiredGet(&actuatorDesired);
555 
556  if (isfinite(actuatorDesired.Roll) && isfinite(actuatorDesired.Pitch)
557  && isfinite(actuatorDesired.Yaw)
558  && isfinite(actuatorDesired.Thrust)) {
559  cmd.roll = actuatorDesired.Roll;
560  cmd.pitch = actuatorDesired.Pitch;
561  cmd.yaw = actuatorDesired.Yaw;
562 
563  if (actuatorDesired.Thrust >= 0) {
564  cmd.throttle = actuatorDesired.Thrust;
565  } else {
566  cmd.throttle = 0;
567  }
568  } else {
569  /* Work around a bug in attitude where things are going NaN
570  * initially-- still needs troubleshooting.
571  */
572  cmd.roll = 0;
573  cmd.pitch = 0;
574  cmd.yaw = 0;
575  cmd.throttle = 0;
576  }
577 
578  cmd.armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
579 
580  ret = write(wfd, &cmd, sizeof(cmd));
581 
582  if (ret != sizeof(cmd)) {
583  fprintf(stderr, "failed: write-to-yasim %d\n", ret);
584  exit(1);
585  }
586 
587  simsensors_gyro_set(&status.p, GYRO_NOISE_SCALE, 30);
588  simsensors_accels_set(status.acc, accel_bias, 31);
589 
590  float rpy[3] = { status.roll * RAD2DEG, status.pitch * RAD2DEG,
591  status.hdg * RAD2DEG };
592 
593  float q[4];
594 
595  RPY2Quaternion(rpy, q);
596 
597  float Rbe[3][3];
598 
599  Quaternion2R(q, Rbe);
600 
601  HomeLocationData homeLocation;
602  HomeLocationGet(&homeLocation);
603  if (homeLocation.Set == HOMELOCATION_SET_FALSE) {
604  homeLocation.Be[0] = 100;
605  homeLocation.Be[1] = 0;
606  homeLocation.Be[2] = 400;
607  homeLocation.Set = HOMELOCATION_SET_TRUE;
608 
609  HomeLocationSet(&homeLocation);
610  }
611 
612  uint32_t last_mag_time = 0;
613  if (PIOS_Thread_Period_Elapsed(last_mag_time, MAG_PERIOD)) {
614  simsensors_mag_set(homeLocation.Be, &Rbe);
615 
616  last_mag_time = PIOS_Thread_Systime();
617  }
618 
619  simsensors_baro_drift(&baro_offset);
620 
621  // Update baro periodically
622  static uint32_t last_baro_time = 0;
623  if (PIOS_Thread_Period_Elapsed(last_baro_time, BARO_PERIOD)) {
624  simsensors_baro_set(status.alt, baro_offset);
625  last_baro_time = PIOS_Thread_Systime();
626  }
627 
628  // Update GPS periodically
629  static uint32_t last_gps_time = 0;
630  static float gps_vel_drift[3] = {0,0,0};
631  if (PIOS_Thread_Period_Elapsed(last_gps_time, GPS_PERIOD)) {
632  // Use double precision here as simulating what GPS produces
633  double T[3];
634  T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
635  T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
636  T[2] = -1.0;
637 
638  static float gps_drift[3] = {0,0,0};
639  gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
640  gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
641  gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
642 
643  GPSPositionData gpsPosition;
644  GPSPositionGet(&gpsPosition);
645  gpsPosition.Latitude = (status.lat + gps_drift[0] / T[0]) * 10.0e6;
646  gpsPosition.Longitude = (status.lon + gps_drift[1] / T[1]) * 10.0e6;
647  gpsPosition.Altitude = (status.alt + gps_drift[2]) / T[2];
648  gpsPosition.Groundspeed = sqrtf(pow(status.vel[0] + gps_vel_drift[0],2) + pow(status.vel[1] + gps_vel_drift[1],2));
649  gpsPosition.Heading = 180 / M_PI * atan2f(status.vel[1] + gps_vel_drift[1], status.vel[0] + gps_vel_drift[0]);
650  gpsPosition.Satellites = 7;
651  gpsPosition.PDOP = 1;
652  gpsPosition.Accuracy = 3.0;
653  gpsPosition.Status = GPSPOSITION_STATUS_FIX3D;
654  GPSPositionSet(&gpsPosition);
655  last_gps_time = PIOS_Thread_Systime();
656  }
657 
658  // Update GPS Velocity measurements
659  static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
660  if (PIOS_Thread_Period_Elapsed(last_gps_vel_time, GPS_PERIOD)) {
661  gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
662  gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
663  gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
664 
665  GPSVelocityData gpsVelocity;
666  GPSVelocityGet(&gpsVelocity);
667  gpsVelocity.North = status.vel[0] + gps_vel_drift[0];
668  gpsVelocity.East = status.vel[1] + gps_vel_drift[1];
669  gpsVelocity.Down = status.vel[2] + gps_vel_drift[2];
670  gpsVelocity.Accuracy = 0.75;
671  GPSVelocitySet(&gpsVelocity);
672  last_gps_vel_time = PIOS_Thread_Systime();
673  }
674 
675  AttitudeSimulatedData attitudeSimulated;
676  AttitudeSimulatedGet(&attitudeSimulated);
677  attitudeSimulated.q1 = q[0];
678  attitudeSimulated.q2 = q[1];
679  attitudeSimulated.q3 = q[2];
680  attitudeSimulated.q4 = q[3];
681  Quaternion2RPY(q, &attitudeSimulated.Roll);
682 
683  attitudeSimulated.Position[0] = status.lat;
684  attitudeSimulated.Position[1] = status.lon;
685  attitudeSimulated.Position[2] = status.alt;
686 
687  attitudeSimulated.Velocity[0] = status.vel[0];
688  attitudeSimulated.Velocity[1] = status.vel[1];
689  attitudeSimulated.Velocity[2] = status.vel[2];
690 
691  AttitudeSimulatedSet(&attitudeSimulated);
692 }
693 #endif /* PIOS_INCLUDE_SIMSENSORS_YASIM */
694 
695 static void simulateModelQuadcopter()
696 {
697  static double pos[3] = {0,0,0};
698  static double vel[3] = {0,0,0};
699  static double ned_accel[3] = {0,0,0};
700  static float q[4] = {1,0,0,0};
701  static float rpy[3] = {0,0,0}; // Low pass filtered actuator
702  static float baro_offset = 0.0f;
703  float Rbe[3][3];
704 
705  const float MAX_THRUST = GRAVITY * 2;
706  const float K_FRICTION = 1;
707  const float GPS_PERIOD = 0.1f;
708  const float MAG_PERIOD = 1.0 / 75.0;
709  const float BARO_PERIOD = 1.0 / 20.0;
710  const float GYRO_NOISE_SCALE = 1.0f;
711 
712  float dT = 0.002;
713  float thrust;
714 
715  simsensors_scale_controls(rpy, &thrust, MAX_THRUST);
716  simsensors_gyro_set(rpy, GYRO_NOISE_SCALE, 20);
717 
718  // Predict the attitude forward in time
719  float qdot[4];
720  qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
721  qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
722  qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
723  qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
724 
725  simsensors_quat_timestep(q, qdot);
726 
727  static float wind[3] = {0,0,0};
728  wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
729  wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
730  wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
731 
732  Quaternion2R(q,Rbe);
733  // Make thrust negative as down is positive
734  ned_accel[0] = -thrust * Rbe[2][0];
735  ned_accel[1] = -thrust * Rbe[2][1];
736  // Gravity causes acceleration of 9.81 in the down direction
737  ned_accel[2] = -thrust * Rbe[2][2] + GRAVITY;
738 
739  // Apply acceleration based on velocity
740  ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
741  ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
742  ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
743 
744  // Predict the velocity forward in time
745  vel[0] = vel[0] + ned_accel[0] * dT;
746  vel[1] = vel[1] + ned_accel[1] * dT;
747  vel[2] = vel[2] + ned_accel[2] * dT;
748 
749  // Predict the position forward in time
750  pos[0] = pos[0] + vel[0] * dT;
751  pos[1] = pos[1] + vel[1] * dT;
752  pos[2] = pos[2] + vel[2] * dT;
753 
754  // Simulate hitting ground
755  if(pos[2] > 0) {
756  pos[2] = 0;
757  vel[2] = 0;
758  ned_accel[2] = 0;
759  }
760 
761  // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
762  ned_accel[2] -= GRAVITY;
763 
764  simsensors_accels_setfromned(ned_accel, &Rbe, accel_bias, 30);
765 
766  simsensors_baro_drift(&baro_offset);
767 
768  // Update baro periodically
769  static uint32_t last_baro_time = 0;
770  if (PIOS_Thread_Period_Elapsed(last_baro_time, BARO_PERIOD)) {
771  simsensors_baro_set(pos[2], baro_offset);
772  last_baro_time = PIOS_Thread_Systime();
773  }
774 
775  HomeLocationData homeLocation;
776  HomeLocationGet(&homeLocation);
777  if (homeLocation.Set == HOMELOCATION_SET_FALSE) {
778  homeLocation.Be[0] = 100;
779  homeLocation.Be[1] = 0;
780  homeLocation.Be[2] = 400;
781  homeLocation.Set = HOMELOCATION_SET_TRUE;
782 
783  HomeLocationSet(&homeLocation);
784  }
785 
786  static float gps_vel_drift[3] = {0,0,0};
787  gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
788  gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
789  gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
790 
791  // Update GPS periodically
792  static uint32_t last_gps_time = 0;
793  if (PIOS_Thread_Period_Elapsed(last_gps_time, GPS_PERIOD)) {
794  // Use double precision here as simulating what GPS produces
795  double T[3];
796  T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
797  T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
798  T[2] = -1.0;
799 
800  static float gps_drift[3] = {0,0,0};
801  gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
802  gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
803  gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
804 
805  GPSPositionData gpsPosition;
806  GPSPositionGet(&gpsPosition);
807  gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
808  gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
809  gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
810  gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
811  gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
812  gpsPosition.Satellites = 7;
813  gpsPosition.PDOP = 1;
814  gpsPosition.Accuracy = 3.0;
815  gpsPosition.Status = GPSPOSITION_STATUS_FIX3D;
816  GPSPositionSet(&gpsPosition);
817  last_gps_time = PIOS_Thread_Systime();
818  }
819 
820  // Update GPS Velocity measurements
821  static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
822  if (PIOS_Thread_Period_Elapsed(last_gps_vel_time, GPS_PERIOD)) {
823  GPSVelocityData gpsVelocity;
824  GPSVelocityGet(&gpsVelocity);
825  gpsVelocity.North = vel[0] + gps_vel_drift[0];
826  gpsVelocity.East = vel[1] + gps_vel_drift[1];
827  gpsVelocity.Down = vel[2] + gps_vel_drift[2];
828  gpsVelocity.Accuracy = 0.75;
829  GPSVelocitySet(&gpsVelocity);
830  last_gps_vel_time = PIOS_Thread_Systime();
831  }
832 
833  // Update mag periodically
834  static uint32_t last_mag_time = 0;
835  if (PIOS_Thread_Period_Elapsed(last_mag_time, MAG_PERIOD)) {
836  simsensors_mag_set(homeLocation.Be, &Rbe);
837  last_mag_time = PIOS_Thread_Systime();
838  }
839 
840  AttitudeSimulatedData attitudeSimulated;
841  AttitudeSimulatedGet(&attitudeSimulated);
842  attitudeSimulated.q1 = q[0];
843  attitudeSimulated.q2 = q[1];
844  attitudeSimulated.q3 = q[2];
845  attitudeSimulated.q4 = q[3];
846  Quaternion2RPY(q,&attitudeSimulated.Roll);
847  attitudeSimulated.Position[0] = pos[0];
848  attitudeSimulated.Position[1] = pos[1];
849  attitudeSimulated.Position[2] = pos[2];
850  attitudeSimulated.Velocity[0] = vel[0];
851  attitudeSimulated.Velocity[1] = vel[1];
852  attitudeSimulated.Velocity[2] = vel[2];
853  AttitudeSimulatedSet(&attitudeSimulated);
854 }
855 
866 static void simulateModelAirplane()
867 {
868  static double pos[3] = {0,0,0};
869  static double vel[3] = {0,0,0};
870  static double ned_accel[3] = {0,0,0};
871  static float q[4] = {1,0,0,0};
872  static float rpy[3] = {0,0,0}; // Low pass filtered actuator
873  static float baro_offset = 0.0f;
874  float Rbe[3][3];
875 
876  const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch
877  const float MAX_THRUST = 9.81 * 2;
878  const float K_FRICTION = 0.2;
879  const float GPS_PERIOD = 0.1;
880  const float MAG_PERIOD = 1.0 / 75.0;
881  const float BARO_PERIOD = 1.0 / 20.0;
882  const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll
883  const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch
884  const float GYRO_NOISE_SCALE = 1.0f;
885 
886  float dT = 0.002;
887  float thrust;
888 
889  /**** 1. Update attitude ****/
890  // Need to get roll angle for easy cross coupling
891  // TODO: Uses the FC's idea of attitude for cross coupling.
892  AttitudeActualData attitudeActual;
893  AttitudeActualGet(&attitudeActual);
894  double roll = attitudeActual.Roll;
895  double pitch = attitudeActual.Pitch;
896 
897  simsensors_scale_controls(rpy, &thrust, MAX_THRUST);
898  rpy[2] += roll * ROLL_HEADING_COUPLING;
899 
900  simsensors_gyro_set(rpy, GYRO_NOISE_SCALE, 20);
901 
902  // Predict the attitude forward in time
903  float qdot[4];
904  qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
905  qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
906  qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
907  qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
908 
909  simsensors_quat_timestep(q, qdot);
910 
911  /**** 2. Update position based on velocity ****/
912  static float wind[3] = {0,0,0};
913  wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
914  wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
915  wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
916  wind[0] = 0;
917  wind[1] = 0;
918  wind[2] = 0;
919 
920  // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed
921  // we get forward airspeed
922  Quaternion2R(q,Rbe);
923 
924  double airspeed[3] = {vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2]};
925  double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2];
926  double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2];
927  double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2];
928 
929  assert(isfinite(forwardAirspeed));
930  assert(isfinite(sidewaysAirspeed));
931  assert(isfinite(downwardAirspeed));
932 
933  AirspeedActualData airspeedObj;
934  airspeedObj.CalibratedAirspeed = forwardAirspeed;
935  // TODO: Factor in temp and pressure when simulated for true airspeed.
936  // This assume standard temperature and pressure which will be inaccurate
937  // at higher altitudes (http://en.wikipedia.org/wiki/Airspeed)
938  airspeedObj.TrueAirspeed = forwardAirspeed;
939  AirspeedActualSet(&airspeedObj);
940 
941  /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */
942  /* TODO: This should become more accurate. Use the force equations to calculate lift from the */
943  /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */
944  double forces[3]; // X, Y, Z
945  forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION; // Friction is applied in all directions in NED
946  forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100; // No side slip
947  forces[2] = GRAVITY * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100; // Stupidly simple, always have gravity lift when straight and level
948 
949  // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
950  ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
951  ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
952  ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2];
953  // Gravity causes acceleration of 9.81 in the down direction
954  ned_accel[2] += 9.81;
955 
956  // Apply acceleration based on velocity
957  ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
958  ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
959  ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
960 
961  // Predict the velocity forward in time
962  vel[0] = vel[0] + ned_accel[0] * dT;
963  vel[1] = vel[1] + ned_accel[1] * dT;
964  vel[2] = vel[2] + ned_accel[2] * dT;
965 
966  assert(isfinite(vel[0]));
967  assert(isfinite(vel[1]));
968  assert(isfinite(vel[2]));
969 
970  // Predict the position forward in time
971  pos[0] = pos[0] + vel[0] * dT;
972  pos[1] = pos[1] + vel[1] * dT;
973  pos[2] = pos[2] + vel[2] * dT;
974 
975  assert(isfinite(pos[0]));
976  assert(isfinite(pos[1]));
977  assert(isfinite(pos[2]));
978 
979  // Simulate hitting ground
980  if(pos[2] > 0) {
981  pos[2] = 0;
982  vel[2] = 0;
983  ned_accel[2] = 0;
984  }
985 
986  // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
987  ned_accel[2] -= GRAVITY;
988 
989  simsensors_accels_setfromned(ned_accel, &Rbe, accel_bias, 30);
990 
991  simsensors_baro_drift(&baro_offset);
992 
993  // Update baro periodically
994  static uint32_t last_baro_time = 0;
995  if (PIOS_Thread_Period_Elapsed(last_baro_time, BARO_PERIOD)) {
996  simsensors_baro_set(pos[2], baro_offset);
997  last_baro_time = PIOS_Thread_Systime();
998  }
999 
1000  // Update baro airpseed periodically
1001  static uint32_t last_airspeed_time = 0;
1002  if (PIOS_Thread_Period_Elapsed(last_airspeed_time, BARO_PERIOD)) {
1003  BaroAirspeedData baroAirspeed;
1004  baroAirspeed.BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
1005  baroAirspeed.CalibratedAirspeed = forwardAirspeed;
1006  baroAirspeed.GPSAirspeed = forwardAirspeed;
1007  baroAirspeed.TrueAirspeed = forwardAirspeed;
1008  BaroAirspeedSet(&baroAirspeed);
1009  last_airspeed_time = PIOS_Thread_Systime();
1010  }
1011 
1012  HomeLocationData homeLocation;
1013  HomeLocationGet(&homeLocation);
1014  if (homeLocation.Set == HOMELOCATION_SET_FALSE) {
1015  homeLocation.Be[0] = 100;
1016  homeLocation.Be[1] = 0;
1017  homeLocation.Be[2] = 400;
1018  homeLocation.Set = HOMELOCATION_SET_TRUE;
1019 
1020  HomeLocationSet(&homeLocation);
1021  }
1022 
1023  static float gps_vel_drift[3] = {0,0,0};
1024  gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
1025  gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
1026  gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
1027 
1028  // Update GPS periodically
1029  static uint32_t last_gps_time = 0;
1030  if (PIOS_Thread_Period_Elapsed(last_gps_time, GPS_PERIOD)) {
1031  // Use double precision here as simulating what GPS produces
1032  double T[3];
1033  T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
1034  T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
1035  T[2] = -1.0;
1036 
1037  static float gps_drift[3] = {0,0,0};
1038  gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
1039  gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
1040  gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
1041 
1042  GPSPositionData gpsPosition;
1043  GPSPositionGet(&gpsPosition);
1044  gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
1045  gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
1046  gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
1047  gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
1048  gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
1049  gpsPosition.Satellites = 7;
1050  gpsPosition.PDOP = 1;
1051  GPSPositionSet(&gpsPosition);
1052  last_gps_time = PIOS_Thread_Systime();
1053  }
1054 
1055  // Update GPS Velocity measurements
1056  static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
1057  if (PIOS_Thread_Period_Elapsed(last_gps_vel_time, GPS_PERIOD)) {
1058  GPSVelocityData gpsVelocity;
1059  GPSVelocityGet(&gpsVelocity);
1060  gpsVelocity.North = vel[0] + gps_vel_drift[0];
1061  gpsVelocity.East = vel[1] + gps_vel_drift[1];
1062  gpsVelocity.Down = vel[2] + gps_vel_drift[2];
1063  GPSVelocitySet(&gpsVelocity);
1064  last_gps_vel_time = PIOS_Thread_Systime();
1065  }
1066 
1067  // Update mag periodically
1068  static uint32_t last_mag_time = 0;
1069  if (PIOS_Thread_Period_Elapsed(last_mag_time, MAG_PERIOD)) {
1070  simsensors_mag_set(homeLocation.Be, &Rbe);
1071  last_mag_time = PIOS_Thread_Systime();
1072  }
1073 
1074  AttitudeSimulatedData attitudeSimulated;
1075  AttitudeSimulatedGet(&attitudeSimulated);
1076  attitudeSimulated.q1 = q[0];
1077  attitudeSimulated.q2 = q[1];
1078  attitudeSimulated.q3 = q[2];
1079  attitudeSimulated.q4 = q[3];
1080  Quaternion2RPY(q,&attitudeSimulated.Roll);
1081  attitudeSimulated.Position[0] = pos[0];
1082  attitudeSimulated.Position[1] = pos[1];
1083  attitudeSimulated.Position[2] = pos[2];
1084  attitudeSimulated.Velocity[0] = vel[0];
1085  attitudeSimulated.Velocity[1] = vel[1];
1086  attitudeSimulated.Velocity[2] = vel[2];
1087  AttitudeSimulatedSet(&attitudeSimulated);
1088 }
1089 
1100 static void simulateModelCar()
1101 {
1102  static double pos[3] = {0,0,0};
1103  static double vel[3] = {0,0,0};
1104  static double ned_accel[3] = {0,0,0};
1105  static float q[4] = {1,0,0,0};
1106  static float rpy[3] = {0,0,0}; // Low pass filtered actuator
1107  static float baro_offset = 0.0f;
1108  float Rbe[3][3];
1109 
1110  const float MAX_THRUST = 9.81 * 0.5;
1111  const float K_FRICTION = 0.2;
1112  const float GPS_PERIOD = 0.1;
1113  const float MAG_PERIOD = 1.0 / 75.0;
1114  const float BARO_PERIOD = 1.0 / 20.0;
1115  const float GYRO_NOISE_SCALE = 1.0;
1116 
1117  float dT = 0.002;
1118  float thrust;
1119 
1120  FlightStatusData flightStatus;
1121  FlightStatusGet(&flightStatus);
1122  ActuatorDesiredData actuatorDesired;
1123  ActuatorDesiredGet(&actuatorDesired);
1124 
1125  /**** 1. Update attitude ****/
1126  simsensors_scale_controls(rpy, &thrust, MAX_THRUST);
1127 
1128  /* Can only yaw */
1129  rpy[0] = 0;
1130  rpy[1] = 0;
1131 
1132  simsensors_gyro_set(rpy, GYRO_NOISE_SCALE, 20);
1133 
1134  // Predict the attitude forward in time
1135  float qdot[4];
1136  qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
1137  qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
1138  qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
1139  qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
1140 
1141  simsensors_quat_timestep(q, qdot);
1142 
1143  /**** 2. Update position based on velocity ****/
1144  // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed
1145  // we get forward airspeed
1146  Quaternion2R(q,Rbe);
1147 
1148  double groundspeed[3] = {vel[0], vel[1], vel[2] };
1149  double forwardSpeed = Rbe[0][0] * groundspeed[0] + Rbe[0][1] * groundspeed[1] + Rbe[0][2] * groundspeed[2];
1150  double sidewaysSpeed = Rbe[1][0] * groundspeed[0] + Rbe[1][1] * groundspeed[1] + Rbe[1][2] * groundspeed[2];
1151 
1152  double forces[3]; // X, Y, Z
1153  forces[0] = thrust - forwardSpeed * K_FRICTION; // Friction is applied in all directions in NED
1154  forces[1] = 0 - sidewaysSpeed * K_FRICTION * 100; // No side slip
1155  forces[2] = 0;
1156 
1157  // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
1158  ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
1159  ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
1160  ned_accel[2] = 0;
1161 
1162  // Apply acceleration based on velocity
1163  ned_accel[0] -= K_FRICTION * (vel[0]);
1164  ned_accel[1] -= K_FRICTION * (vel[1]);
1165 
1166  // Predict the velocity forward in time
1167  vel[0] = vel[0] + ned_accel[0] * dT;
1168  vel[1] = vel[1] + ned_accel[1] * dT;
1169  vel[2] = vel[2] + ned_accel[2] * dT;
1170 
1171  // Predict the position forward in time
1172  pos[0] = pos[0] + vel[0] * dT;
1173  pos[1] = pos[1] + vel[1] * dT;
1174  pos[2] = pos[2] + vel[2] * dT;
1175 
1176  // Simulate hitting ground
1177  if(pos[2] > 0) {
1178  pos[2] = 0;
1179  vel[2] = 0;
1180  ned_accel[2] = 0;
1181  }
1182 
1183  // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
1184  ned_accel[2] -= GRAVITY;
1185 
1186  // Transform the accels back in to body frame
1187  simsensors_accels_setfromned(ned_accel, &Rbe, accel_bias, 30);
1188 
1189  simsensors_baro_drift(&baro_offset);
1190 
1191  // Update baro periodically
1192  static uint32_t last_baro_time = 0;
1193  if (PIOS_Thread_Period_Elapsed(last_baro_time, BARO_PERIOD)) {
1194  simsensors_baro_set(pos[2], baro_offset);
1195  last_baro_time = PIOS_Thread_Systime();
1196  }
1197 
1198  HomeLocationData homeLocation;
1199  HomeLocationGet(&homeLocation);
1200  if (homeLocation.Set == HOMELOCATION_SET_FALSE) {
1201  homeLocation.Be[0] = 100;
1202  homeLocation.Be[1] = 0;
1203  homeLocation.Be[2] = 400;
1204  homeLocation.Set = HOMELOCATION_SET_TRUE;
1205 
1206  HomeLocationSet(&homeLocation);
1207  }
1208 
1209  static float gps_vel_drift[3] = {0,0,0};
1210  gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
1211  gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
1212  gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
1213 
1214  // Update GPS periodically
1215  static uint32_t last_gps_time = 0;
1216  if (PIOS_Thread_Period_Elapsed(last_gps_time, GPS_PERIOD)) {
1217  // Use double precision here as simulating what GPS produces
1218  double T[3];
1219  T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
1220  T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
1221  T[2] = -1.0;
1222 
1223  static float gps_drift[3] = {0,0,0};
1224  gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
1225  gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
1226  gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
1227 
1228  GPSPositionData gpsPosition;
1229  GPSPositionGet(&gpsPosition);
1230  gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
1231  gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
1232  gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
1233  gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
1234  gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
1235  gpsPosition.Satellites = 7;
1236  gpsPosition.PDOP = 1;
1237  GPSPositionSet(&gpsPosition);
1238  last_gps_time = PIOS_Thread_Systime();
1239  }
1240 
1241  // Update GPS Velocity measurements
1242  static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
1243  if (PIOS_Thread_Period_Elapsed(last_gps_vel_time, GPS_PERIOD)) {
1244  GPSVelocityData gpsVelocity;
1245  GPSVelocityGet(&gpsVelocity);
1246  gpsVelocity.North = vel[0] + gps_vel_drift[0];
1247  gpsVelocity.East = vel[1] + gps_vel_drift[1];
1248  gpsVelocity.Down = vel[2] + gps_vel_drift[2];
1249  GPSVelocitySet(&gpsVelocity);
1250  last_gps_vel_time = PIOS_Thread_Systime();
1251  }
1252 
1253  // Update mag periodically
1254  static uint32_t last_mag_time = 0;
1255  if (PIOS_Thread_Period_Elapsed(last_mag_time, MAG_PERIOD)) {
1256  simsensors_mag_set(homeLocation.Be, &Rbe);
1257  last_mag_time = PIOS_Thread_Systime();
1258  }
1259 
1260  AttitudeSimulatedData attitudeSimulated;
1261  AttitudeSimulatedGet(&attitudeSimulated);
1262  attitudeSimulated.q1 = q[0];
1263  attitudeSimulated.q2 = q[1];
1264  attitudeSimulated.q3 = q[2];
1265  attitudeSimulated.q4 = q[3];
1266  Quaternion2RPY(q,&attitudeSimulated.Roll);
1267  attitudeSimulated.Position[0] = pos[0];
1268  attitudeSimulated.Position[1] = pos[1];
1269  attitudeSimulated.Position[2] = pos[2];
1270  attitudeSimulated.Velocity[0] = vel[0];
1271  attitudeSimulated.Velocity[1] = vel[1];
1272  attitudeSimulated.Velocity[2] = vel[2];
1273  AttitudeSimulatedSet(&attitudeSimulated);
1274 }
1275 
1276 
1277 static float rand_gauss (void) {
1278  float v1,v2,s;
1279 
1280  do {
1281  v1 = 2.0 * ((float) rand()/RAND_MAX) - 1;
1282  v2 = 2.0 * ((float) rand()/RAND_MAX) - 1;
1283 
1284  s = v1*v1 + v2*v2;
1285  } while ( s >= 1.0 );
1286 
1287  if (s == 0.0)
1288  return 0.0;
1289  else
1290  return (v1*sqrtf(-2.0 * log(s) / s));
1291 }
1292 
1293 #endif /* PIOS_INCLUDE_SIMSENSORS */
1294 
PIOS_Thread_Systime
uint32_t PIOS_Thread_Systime(void)
Definition: pios_thread.c:212
PIOS_SENSOR_GYRO
Definition: pios_sensors.h:88
PIOS_SENSORS_SetMaxGyro
void PIOS_SENSORS_SetMaxGyro(int32_t rate)
Set the maximum gyro rate in deg/s.
Definition: pios_sensors.c:162
pios.h
Main PiOS header to include all the compiled in PiOS options.
Quaternion2R
void Quaternion2R(float q[4], float Rbe[3][3])
Definition: coordinate_conversions.c:137
RPY2Quaternion
void RPY2Quaternion(const float rpy[3], float q[4])
Definition: coordinate_conversions.c:108
pitch
struct _msp_pid_item pitch
Definition: msp_messages.h:97
roll
struct _msp_pid_item roll
Definition: msp_messages.h:96
lon
uint32_t lon
Definition: msp_messages.h:98
lat
uint32_t lat
Definition: msp_messages.h:97
inited
static bool inited
Definition: main.c:47
PIOS_SENSOR_BARO
Definition: pios_sensors.h:90
flags
uint16_t flags
Definition: uavtalk_priv.h:52
T
static float T[3]
Scales used in NED transform (local tangent plane approx).
Definition: attitude.c:210
PIOS_SENSOR_ACCEL
Definition: pios_sensors.h:87
accel_bias
static float accel_bias[3]
Definition: sensors.c:114
status
int16_t status
Definition: main.c:61
PIOS_SENSOR_MAG
Definition: pios_sensors.h:89
alt
struct _msp_pid_item alt
Definition: msp_messages.h:99
pios_sensor_gyro_data::temperature
float temperature
Definition: pios_sensors.h:42
PIOS_SENSORS_IsRegistered
bool PIOS_SENSORS_IsRegistered(enum pios_sensor_type type)
Checks if a sensor type is registered with the PIOS_SENSORS interface.
Definition: pios_sensors.c:88
pios_sensor_accel_data::temperature
float temperature
Definition: pios_sensors.h:50
pos
struct _msp_pid_item pos
Definition: msp_messages.h:100
pios_sensor_accel_data
Pios sensor structure for generic accel data.
Definition: pios_sensors.h:46
pios_sensor_baro_data::temperature
float temperature
Definition: pios_sensors.h:79
groundspeed
uint16_t groundspeed
Definition: pios_crossfire.h:43
pios_sensor_baro_data
Pios sensor structure for generic baro data.
Definition: pios_sensors.h:78
i
uint8_t i
Definition: msp_messages.h:97
yaw
struct _msp_pid_item yaw
Definition: msp_messages.h:98
homeLocation
static HomeLocationData homeLocation
Definition: attitude.c:147
PIOS_SENSORS_RegisterCallback
int32_t PIOS_SENSORS_RegisterCallback(enum pios_sensor_type type, PIOS_SENSOR_Callback_t callback, void *ctx)
Register a callback-based sensor with the PIOS_SENSORS interface.
Definition: pios_sensors.c:68
pios_sensor_gyro_data
Pios sensor structure for generic gyro data.
Definition: pios_sensors.h:38
PIOS_Thread_Period_Elapsed
bool PIOS_Thread_Period_Elapsed(const uint32_t prev_systime, const uint32_t increment_ms)
Determine if a period has elapsed since a datum.
Definition: pios_thread.c:281
magic
uint32_t magic
Definition: pios_flashfs_logfs.c:72
coordinate_conversions.h
Header for Coordinate conversions library in coordinate_conversions.c.
pios_sensor_mag_data
Pios sensor structure for generic mag data.
Definition: pios_sensors.h:54
px_mkfw.f
tuple f
Definition: px_mkfw.py:81
PIOS_SENSORS_SetSampleRate
void PIOS_SENSORS_SetSampleRate(enum pios_sensor_type type, uint32_t sample_rate)
Set the sample rate of a sensor (Hz)
Definition: pios_sensors.c:172
openpilot.h
Includes PiOS and core architecture components.
AlarmsClear
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
Definition: alarms.c:171
printf
int printf(const char *format,...)
Definition: printf-stdarg.c:201
if
if(BaroAltitudeHandle()!=NULL)
Definition: UAVOFrSKYSensorHubBridge.c:363
p
uint8_t p
Definition: msp_messages.h:96
cmd
static ManualControlCommandData cmd
Definition: transmitter_control.c:88
Quaternion2RPY
void Quaternion2RPY(const float q[4], float rpy[3])
Definition: coordinate_conversions.c:62
pios_thread.h
vel
struct _msp_pid_item vel
Definition: msp_messages.h:105