From bf1f27af32ea0af465a05c05994300c7849ea401 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 May 2021 10:25:12 +1000 Subject: [PATCH] SITL: added is_lock_step_scheduled() API used to fix panic on bad timing --- libraries/SITL/SIM_Aircraft.cpp | 1 + libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SIM_BalanceBot.cpp | 1 + libraries/SITL/SIM_Blimp.cpp | 1 + libraries/SITL/SIM_Helicopter.cpp | 1 + libraries/SITL/SIM_Multicopter.cpp | 1 + libraries/SITL/SIM_Plane.cpp | 3 ++- libraries/SITL/SIM_QuadPlane.cpp | 1 + libraries/SITL/SIM_Rover.cpp | 1 + libraries/SITL/SIM_Sailboat.cpp | 1 + libraries/SITL/SIM_SingleCopter.cpp | 1 + libraries/SITL/SIM_Submarine.cpp | 1 + libraries/SITL/SITL.h | 2 ++ 13 files changed, 15 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 1a09de5214..a30e483703 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -330,6 +330,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) // initialise home fdm.home = home; } + fdm.is_lock_step_scheduled = lock_step_scheduled; fdm.latitude = location.lat * 1.0e-7; fdm.longitude = location.lng * 1.0e-7; fdm.altitude = location.alt * 1.0e-2; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index f099c0b057..eb88f5add6 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -173,6 +173,7 @@ protected: float battery_voltage = -1.0f; float battery_current; float local_ground_level; // ground level at local position + bool lock_step_scheduled; // battery model Battery battery; diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp index fd5412ce1b..d29c28af99 100644 --- a/libraries/SITL/SIM_BalanceBot.cpp +++ b/libraries/SITL/SIM_BalanceBot.cpp @@ -28,6 +28,7 @@ BalanceBot::BalanceBot(const char *frame_str) : skid_turn_rate(0.15708) // meters/sec { dcm.from_euler(0,0,0); // initial yaw, pitch and roll in radians + lock_step_scheduled = true; printf("Balance Bot Simulation Started\n"); } diff --git a/libraries/SITL/SIM_Blimp.cpp b/libraries/SITL/SIM_Blimp.cpp index 06e8d50dc2..faf5d20e4a 100644 --- a/libraries/SITL/SIM_Blimp.cpp +++ b/libraries/SITL/SIM_Blimp.cpp @@ -28,6 +28,7 @@ Blimp::Blimp(const char *frame_str) : { frame_height = 0.0; ground_behavior = GROUND_BEHAVIOR_NONE; + lock_step_scheduled = true; } // calculate rotational and linear accelerations diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index c74ded4eae..db1391130f 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -52,6 +52,7 @@ Helicopter::Helicopter(const char *frame_str) : gas_heli = (strstr(frame_str, "-gas") != nullptr); ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; + lock_step_scheduled = true; } /* diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index ae187bac55..e271849587 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -38,6 +38,7 @@ MultiCopter::MultiCopter(const char *frame_str) : frame_height = 0.1; num_motors = frame->num_motors; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; + lock_step_scheduled = true; } // calculate rotational and linear accelerations diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 7ab61159cd..0fd94418d5 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -37,7 +37,8 @@ Plane::Plane(const char *frame_str) : num_motors = 1; ground_behavior = GROUND_BEHAVIOR_FWD_ONLY; - + lock_step_scheduled = true; + if (strstr(frame_str, "-heavy")) { mass = 8; } diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 8127d0c0e7..371d914e9b 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -95,6 +95,7 @@ QuadPlane::QuadPlane(const char *frame_str) : frame->set_mass(mass); ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; + lock_step_scheduled = true; } /* diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index 640e54f542..81a5eda8b1 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -41,6 +41,7 @@ SimRover::SimRover(const char *frame_str) : if (vectored_thrust) { printf("Vectored Thrust Rover Simulation Started\n"); } + lock_step_scheduled = true; } diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp index d24955656a..6f6b26b1f6 100644 --- a/libraries/SITL/SIM_Sailboat.cpp +++ b/libraries/SITL/SIM_Sailboat.cpp @@ -46,6 +46,7 @@ Sailboat::Sailboat(const char *frame_str) : sail_area(1.0) { motor_connected = (strcmp(frame_str, "sailboat-motor") == 0); + lock_step_scheduled = true; } // calculate the lift and drag as values from 0 to 1 diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp index a4937976c4..d12699c4b8 100644 --- a/libraries/SITL/SIM_SingleCopter.cpp +++ b/libraries/SITL/SIM_SingleCopter.cpp @@ -39,6 +39,7 @@ SingleCopter::SingleCopter(const char *frame_str) : */ thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; frame_height = 0.1; + lock_step_scheduled = true; } /* diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index 69401bf6b2..a245706134 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -62,6 +62,7 @@ Submarine::Submarine(const char *frame_str) : thrusters = vectored_6dof_thrusters; n_thrusters = 8; } + lock_step_scheduled = true; } // calculate rotational and linear accelerations diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 2e2bad2ffd..dd2c3fee1b 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -81,6 +81,8 @@ struct sitl_fdm { float speed; float direction; } wind_vane_apparent; + + bool is_lock_step_scheduled; }; // number of rc output channels