diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index 87337498f3..640e54f542 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -34,6 +34,12 @@ SimRover::SimRover(const char *frame_str) : // with a sabertooth controller max_accel = 14; max_speed = 4; + return; + } + + vectored_thrust = strstr(frame_str, "vector") != nullptr; + if (vectored_thrust) { + printf("Vectored Thrust Rover Simulation Started\n"); } } @@ -58,6 +64,9 @@ float SimRover::calc_yaw_rate(float steering, float speed) if (skid_steering) { return steering * skid_turn_rate; } + if (vectored_thrust) { + return steering * vectored_turn_rate_max; + } if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) { return 0; } @@ -94,6 +103,13 @@ void SimRover::update(const struct sitl_input &input) } else { steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f); throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f); + + // vectored thrust conversion + if (vectored_thrust) { + const float steering_angle_rad = radians(steering * vectored_angle_max); + steering = sinf(steering_angle_rad) * throttle; + throttle *= cosf(steering_angle_rad); + } } // how much time has passed? diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 3b287579c5..d79481cc19 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -45,6 +45,11 @@ private: float skid_turn_rate = 140.0f; // skid-steering vehicle's maximum turn rate in deg/sec bool skid_steering; // true if this vehicle is a skid-steering vehicle + // vectored thrust related members + bool vectored_thrust; // true if vehicle uses vectored thrust (i.e. steering controls direction of thrust) + float vectored_angle_max = 90.0f; // maximum angle (in degrees) to which thrust can be turned + float vectored_turn_rate_max = 90.0f; // maximum turn rate (in deg/sec) with full throttle angled at 90deg + float turn_circle(float steering) const; float calc_yaw_rate(float steering, float speed); float calc_lat_accel(float steering_angle, float speed);