diff --git a/libraries/SITL/SIM_ICEngine.cpp b/libraries/SITL/SIM_ICEngine.cpp index 4f5d421d92..4217e48de9 100644 --- a/libraries/SITL/SIM_ICEngine.cpp +++ b/libraries/SITL/SIM_ICEngine.cpp @@ -84,9 +84,12 @@ float ICEngine::update(const Aircraft::sitl_input &input) } if (start_time_us != 0 && state.starter) { uint32_t starter_time_us = (now - start_time_us); - if (starter_time_us > 3000*1000UL) { + if (starter_time_us > 3000*1000UL && !overheat) { + overheat = true; printf("Starter overheat\n"); } + } else { + overheat = false; } output: diff --git a/libraries/SITL/SIM_ICEngine.h b/libraries/SITL/SIM_ICEngine.h index 83807fc28b..12d732f57e 100644 --- a/libraries/SITL/SIM_ICEngine.h +++ b/libraries/SITL/SIM_ICEngine.h @@ -54,5 +54,6 @@ private: }; uint8_t value; } state, last_state; + bool overheat:1; }; } diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 0aeb8de721..cdd1f2e505 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -250,6 +250,9 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel Vector3f force = getForce(aileron, elevator, rudder); rot_accel = getTorque(aileron, elevator, rudder, force); + // simulate engine RPM + rpm1 = thrust * 7000; + // scale thrust to newtons thrust *= thrust_scale; diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 3ae78e1fdd..138d9c8386 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -98,7 +98,7 @@ protected: bool reverse_elevator_rudder; bool ice_engine; - ICEngine icengine{2, 4, 5, 6, 100}; + ICEngine icengine{2, 14, 12, 13, 100}; float liftCoeff(float alpha) const; float dragCoeff(float alpha) const;