Browse Source

SITL: remove zero initialisations, move more into class definitions

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
commit
abab203b32
  1. 21
      libraries/SITL/SIM_Aircraft.cpp
  2. 30
      libraries/SITL/SIM_Aircraft.h
  3. 3
      libraries/SITL/SIM_Multicopter.cpp
  4. 8
      libraries/SITL/SIM_Rover.cpp
  5. 10
      libraries/SITL/SIM_Rover.h
  6. 5
      libraries/SITL/SIM_Tracker.cpp
  7. 5
      libraries/SITL/SIM_Tracker.h

21
libraries/SITL/SIM_Aircraft.cpp

@ -41,25 +41,7 @@ using namespace SITL;
*/ */
Aircraft::Aircraft(const char *frame_str) : Aircraft::Aircraft(const char *frame_str) :
ground_level(0.0f), frame(frame_str)
frame_height(0.0f),
dcm(),
gyro(),
velocity_ef(),
mass(0.0f),
accel_body(0.0f, 0.0f, -GRAVITY_MSS),
time_now_us(0),
gyro_noise(radians(0.1f)),
accel_noise(0.3f),
rate_hz(1200.0f),
autotest_dir(nullptr),
frame(frame_str),
num_motors(1),
#if defined(__CYGWIN__) || defined(__CYGWIN64__)
min_sleep_time(20000)
#else
min_sleep_time(5000)
#endif
{ {
// make the SIM_* variables available to simulator backends // make the SIM_* variables available to simulator backends
sitl = AP::sitl(); sitl = AP::sitl();
@ -67,7 +49,6 @@ Aircraft::Aircraft(const char *frame_str) :
set_speedup(1.0f); set_speedup(1.0f);
last_wall_time_us = get_wall_time_us(); last_wall_time_us = get_wall_time_us();
frame_counter = 0;
// allow for orientation settings, such as with tailsitters // allow for orientation settings, such as with tailsitters
enum ap_var_type ptype; enum ap_var_type ptype;

30
libraries/SITL/SIM_Aircraft.h

@ -164,15 +164,15 @@ protected:
Vector3f velocity_air_bf; // velocity relative to airmass, body frame Vector3f velocity_air_bf; // velocity relative to airmass, body frame
Vector3f position; // meters, NED from origin Vector3f position; // meters, NED from origin
float mass; // kg float mass; // kg
float external_payload_mass = 0.0f; // kg float external_payload_mass; // kg
Vector3f accel_body; // m/s/s NED, body frame Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame
float airspeed; // m/s, apparent airspeed float airspeed; // m/s, apparent airspeed
float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube
float battery_voltage = -1.0f; float battery_voltage = -1.0f;
float battery_current = 0.0f; float battery_current;
uint8_t num_motors = 1; uint8_t num_motors = 1;
float rpm[12]; float rpm[12];
uint8_t rcin_chan_count = 0; uint8_t rcin_chan_count;
float rcin[12]; float rcin[12];
float range = -1.0f; // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude float range = -1.0f; // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude
@ -192,17 +192,17 @@ protected:
} wind_vane_apparent; } wind_vane_apparent;
// Wind Turbulence simulated Data // Wind Turbulence simulated Data
float turbulence_azimuth = 0.0f; float turbulence_azimuth;
float turbulence_horizontal_speed = 0.0f; // m/s float turbulence_horizontal_speed; // m/s
float turbulence_vertical_speed = 0.0f; // m/s float turbulence_vertical_speed; // m/s
Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame
uint64_t time_now_us; uint64_t time_now_us;
const float gyro_noise; const float gyro_noise = radians(0.1f);
const float accel_noise; const float accel_noise = 0.3f;
float rate_hz; float rate_hz = 1200.0f;
float target_speedup; float target_speedup;
uint64_t frame_time_us; uint64_t frame_time_us;
uint64_t last_wall_time_us; uint64_t last_wall_time_us;
@ -289,10 +289,14 @@ protected:
float get_local_updraft(Vector3f currentPos); float get_local_updraft(Vector3f currentPos);
private: private:
uint64_t last_time_us = 0; uint64_t last_time_us;
uint32_t frame_counter = 0; uint32_t frame_counter;
uint32_t last_ground_contact_ms; uint32_t last_ground_contact_ms;
const uint32_t min_sleep_time; #if defined(__CYGWIN__) || defined(__CYGWIN64__)
const uint32_t min_sleep_time{20000};
#else
const uint32_t min_sleep_time{5000};
#endif
struct { struct {
Vector3f accel_body; Vector3f accel_body;

3
libraries/SITL/SIM_Multicopter.cpp

@ -24,8 +24,7 @@
using namespace SITL; using namespace SITL;
MultiCopter::MultiCopter(const char *frame_str) : MultiCopter::MultiCopter(const char *frame_str) :
Aircraft(frame_str), Aircraft(frame_str)
frame(nullptr)
{ {
mass = 1.5f; mass = 1.5f;

8
libraries/SITL/SIM_Rover.cpp

@ -24,13 +24,7 @@
namespace SITL { namespace SITL {
SimRover::SimRover(const char *frame_str) : SimRover::SimRover(const char *frame_str) :
Aircraft(frame_str), Aircraft(frame_str)
max_speed(20),
max_accel(10),
max_wheel_turn(35),
turning_circle(1.8),
skid_turn_rate(140), // degrees/sec
skid_steering(false)
{ {
skid_steering = strstr(frame_str, "skid") != nullptr; skid_steering = strstr(frame_str, "skid") != nullptr;

10
libraries/SITL/SIM_Rover.h

@ -38,11 +38,11 @@ public:
} }
private: private:
float max_speed; float max_speed = 20.0f;
float max_accel; float max_accel = 10.0f;
float max_wheel_turn; float max_wheel_turn = 35.0f;
float turning_circle; float turning_circle = 1.8f;
float skid_turn_rate; float skid_turn_rate = 140.0f;
bool skid_steering; bool skid_steering;
float turn_circle(float steering); float turn_circle(float steering);

5
libraries/SITL/SIM_Tracker.cpp

@ -22,11 +22,6 @@
namespace SITL { namespace SITL {
Tracker::Tracker(const char *frame_str) :
Aircraft(frame_str)
{}
/* /*
update function for position (normal) servos. update function for position (normal) servos.
*/ */

5
libraries/SITL/SIM_Tracker.h

@ -27,7 +27,8 @@ namespace SITL {
*/ */
class Tracker : public Aircraft { class Tracker : public Aircraft {
public: public:
Tracker(const char *frame_str); using Aircraft::Aircraft;
void update(const struct sitl_input &input) override; void update(const struct sitl_input &input) override;
/* static object creator */ /* static object creator */
@ -44,7 +45,7 @@ private:
const float yaw_range = 170; const float yaw_range = 170;
const float zero_yaw = 270; // yaw direction at startup const float zero_yaw = 270; // yaw direction at startup
const float zero_pitch = 10; // pitch at startup const float zero_pitch = 10; // pitch at startup
uint64_t last_debug_us = 0; uint64_t last_debug_us;
float pitch_input; float pitch_input;
float yaw_input; float yaw_input;

Loading…
Cancel
Save