diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index a2c46aafaa..9638e348d5 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -35,6 +35,10 @@ Plane::Plane(const char *home_str, const char *frame_str) : */ thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; frame_height = 0.1f; + + if (strstr(frame_str, "-revthrust")) { + reverse_thrust = true; + } } /* @@ -190,7 +194,13 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel float aileron = (input.servos[0]-1500)/500.0f; float elevator = (input.servos[1]-1500)/500.0f; float rudder = (input.servos[3]-1500)/500.0f; - float throttle = constrain_float((input.servos[2]-1000)/1000.0f, 0, 1); + float throttle; + + if (reverse_thrust) { + throttle = constrain_float((input.servos[2]-1500)/500.0f, -1, 1); + } else { + throttle = constrain_float((input.servos[2]-1000)/1000.0f, 0, 1); + } float thrust = throttle; @@ -211,7 +221,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel accel_body += force; // add some noise - add_noise(thrust / thrust_scale); + add_noise(fabsf(thrust) / thrust_scale); } /* diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 9b006df200..0df9133954 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -92,6 +92,7 @@ protected: } coefficient; float thrust_scale; + bool reverse_thrust; float liftCoeff(float alpha) const; float dragCoeff(float alpha) const;