Browse Source

SITL: permit double-precision maths in SITL even on embedded hardware

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
commit
b360521d0b
  1. 3
      libraries/SITL/SIM_Aircraft.cpp
  2. 2
      libraries/SITL/SIM_BalanceBot.cpp
  3. 2
      libraries/SITL/SIM_Blimp.cpp
  4. 2
      libraries/SITL/SIM_GPS.cpp
  5. 2
      libraries/SITL/SIM_MS5611.cpp
  6. 2
      libraries/SITL/SIM_Plane.cpp
  7. 6
      libraries/SITL/SIM_Submarine.h
  8. 2
      libraries/SITL/SITL.cpp

3
libraries/SITL/SIM_Aircraft.cpp

@ -16,13 +16,14 @@ @@ -16,13 +16,14 @@
parent class for aircraft simulators
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SIM_Aircraft.h"
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#if defined(__CYGWIN__) || defined(__CYGWIN64__)
#include <windows.h>
#include <time.h>

2
libraries/SITL/SIM_BalanceBot.cpp

@ -16,6 +16,8 @@ @@ -16,6 +16,8 @@
Balance Bot simulator class
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SIM_BalanceBot.h"
#include <stdio.h>

2
libraries/SITL/SIM_Blimp.cpp

@ -16,6 +16,8 @@ @@ -16,6 +16,8 @@
Blimp simulator class
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SIM_Blimp.h"
#include <stdio.h>

2
libraries/SITL/SIM_GPS.cpp

@ -12,6 +12,8 @@ @@ -12,6 +12,8 @@
#include <time.h>
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>

2
libraries/SITL/SIM_MS5611.cpp

@ -1,3 +1,5 @@ @@ -1,3 +1,5 @@
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SIM_MS5611.h"
#include <SITL/SITL.h>

2
libraries/SITL/SIM_Plane.cpp

@ -17,6 +17,8 @@ @@ -17,6 +17,8 @@
just enough to be able to debug control logic for new frame types
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SIM_Plane.h"
#include <stdio.h>

6
libraries/SITL/SIM_Submarine.h

@ -67,12 +67,12 @@ protected: @@ -67,12 +67,12 @@ protected:
float thruster_mount_radius = 0.25; // distance in meters from thrusters to center of mass. Used to calculate torque.
float equivalent_sphere_radius = 0.2;
// volume = 4.pi.r³/3
float volume = 4 * M_PI * pow(equivalent_sphere_radius, 3) / 3;
float volume = 4 * M_PI * powf(equivalent_sphere_radius, 3) / 3;
float density = 500;
float mass = volume * density; // 16.75 kg
// Moment of Inertia (I)(kg.m²) approximated with a sphere with a 25 cm radius (r) and same density as water
// I = 2.m.r²/5
float moment_of_inertia = 2 * (mass * pow(equivalent_sphere_radius, 2) / 5);
float moment_of_inertia = 2 * (mass * powf(equivalent_sphere_radius, 2) / 5);
// Frame drag coefficient
const Vector3f linear_drag_coefficient = Vector3f(1.4, 1.8, 2.0);
@ -83,7 +83,7 @@ protected: @@ -83,7 +83,7 @@ protected:
// $ V = 4 * pi * r^3 / 3 $
// $ r ^2 = (V * 3 / 4) ^ (2/3) $
// A = area (m^3), r = sphere radius (m)
float equivalent_sphere_area = M_PI_4 * pow(volume * 3.0f / 4.0f, 2.0f / 3.0f);
float equivalent_sphere_area = M_PI_4 * powf(volume * 3.0f / 4.0f, 2.0f / 3.0f);
} frame_property;

2
libraries/SITL/SITL.cpp

@ -17,6 +17,8 @@ @@ -17,6 +17,8 @@
SITL.cpp - software in the loop state
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "SITL.h"
#if AP_SIM_ENABLED

Loading…
Cancel
Save