From abab203b326fb3fc171dd506408135fce3b3dbcb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Sep 2020 08:44:18 +1000 Subject: [PATCH] SITL: remove zero initialisations, move more into class definitions --- libraries/SITL/SIM_Aircraft.cpp | 21 +-------------------- libraries/SITL/SIM_Aircraft.h | 30 +++++++++++++++++------------- libraries/SITL/SIM_Multicopter.cpp | 3 +-- libraries/SITL/SIM_Rover.cpp | 8 +------- libraries/SITL/SIM_Rover.h | 10 +++++----- libraries/SITL/SIM_Tracker.cpp | 5 ----- libraries/SITL/SIM_Tracker.h | 5 +++-- 7 files changed, 28 insertions(+), 54 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 61d1f56d4b..c6607bb18d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -41,25 +41,7 @@ using namespace SITL; */ Aircraft::Aircraft(const char *frame_str) : - ground_level(0.0f), - 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 + frame(frame_str) { // make the SIM_* variables available to simulator backends sitl = AP::sitl(); @@ -67,7 +49,6 @@ Aircraft::Aircraft(const char *frame_str) : set_speedup(1.0f); last_wall_time_us = get_wall_time_us(); - frame_counter = 0; // allow for orientation settings, such as with tailsitters enum ap_var_type ptype; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index f89ede7351..f70cdae626 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -164,15 +164,15 @@ protected: Vector3f velocity_air_bf; // velocity relative to airmass, body frame Vector3f position; // meters, NED from origin float mass; // kg - float external_payload_mass = 0.0f; // kg - Vector3f accel_body; // m/s/s NED, body frame + 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 = 0.0f; + float battery_current; uint8_t num_motors = 1; float rpm[12]; - uint8_t rcin_chan_count = 0; + uint8_t rcin_chan_count; float rcin[12]; 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 Turbulence simulated Data - float turbulence_azimuth = 0.0f; - float turbulence_horizontal_speed = 0.0f; // m/s - float turbulence_vertical_speed = 0.0f; // m/s + 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; - const float accel_noise; - float rate_hz; + 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; @@ -289,10 +289,14 @@ protected: float get_local_updraft(Vector3f currentPos); private: - uint64_t last_time_us = 0; - uint32_t frame_counter = 0; + uint64_t last_time_us; + uint32_t frame_counter; 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 { Vector3f accel_body; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index d45085fe49..2d51cabe35 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -24,8 +24,7 @@ using namespace SITL; MultiCopter::MultiCopter(const char *frame_str) : - Aircraft(frame_str), - frame(nullptr) + Aircraft(frame_str) { mass = 1.5f; diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index cdbe77899c..9d17390af0 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -24,13 +24,7 @@ namespace SITL { SimRover::SimRover(const char *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) + Aircraft(frame_str) { skid_steering = strstr(frame_str, "skid") != nullptr; diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 830fd79922..56d4b1fc09 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -38,11 +38,11 @@ public: } private: - float max_speed; - float max_accel; - float max_wheel_turn; - float turning_circle; - float skid_turn_rate; + float max_speed = 20.0f; + float max_accel = 10.0f; + float max_wheel_turn = 35.0f; + float turning_circle = 1.8f; + float skid_turn_rate = 140.0f; bool skid_steering; float turn_circle(float steering); diff --git a/libraries/SITL/SIM_Tracker.cpp b/libraries/SITL/SIM_Tracker.cpp index 1a52004a01..910787b597 100644 --- a/libraries/SITL/SIM_Tracker.cpp +++ b/libraries/SITL/SIM_Tracker.cpp @@ -22,11 +22,6 @@ namespace SITL { -Tracker::Tracker(const char *frame_str) : -Aircraft(frame_str) -{} - - /* update function for position (normal) servos. */ diff --git a/libraries/SITL/SIM_Tracker.h b/libraries/SITL/SIM_Tracker.h index 82eea5abbb..4f1083dd84 100644 --- a/libraries/SITL/SIM_Tracker.h +++ b/libraries/SITL/SIM_Tracker.h @@ -27,7 +27,8 @@ namespace SITL { */ class Tracker : public Aircraft { public: - Tracker(const char *frame_str); + using Aircraft::Aircraft; + void update(const struct sitl_input &input) override; /* static object creator */ @@ -44,7 +45,7 @@ private: const float yaw_range = 170; const float zero_yaw = 270; // yaw direction 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 yaw_input;