diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 30dd310d76..14d5f7fd6b 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -36,7 +36,6 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
dcm(),
gyro(),
velocity_ef(),
- velocity_body(),
mass(0),
accel_body(0, 0, -GRAVITY_MSS),
time_now_us(0),
@@ -84,7 +83,6 @@ void Aircraft::update_position(void)
location_update(location, bearing, distance);
location.alt = home.alt - position.z*100.0f;
- velocity_body = dcm.transposed() * velocity_ef;
time_now_us += frame_time_us;
sync_frame_time();
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 41baebb1ea..02707155d7 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -62,7 +62,6 @@ protected:
Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
Vector3f gyro; // rad/s
Vector3f velocity_ef; // m/s, earth frame
- Vector3f velocity_body; // m/s, body frame
Vector3f position; // meters, NED from origin
float mass; // kg
Vector3f accel_body; // m/s/s NED, body frame
diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp
new file mode 100644
index 0000000000..87c98af8dc
--- /dev/null
+++ b/libraries/SITL/SIM_Rover.cpp
@@ -0,0 +1,157 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ rover simulator class
+*/
+
+#include "SIM_Rover.h"
+#include
+#include
+
+/*
+ constructor
+ */
+Rover::Rover(const char *home_str, const char *frame_str) :
+ Aircraft(home_str, frame_str),
+ max_speed(20),
+ max_accel(30),
+ wheelbase(0.335),
+ wheeltrack(0.296),
+ max_wheel_turn(35),
+ turning_circle(1.8),
+ skid_turn_rate(140), // degrees/sec
+ skid_steering(false)
+{
+ skid_steering = strstr(frame_str, "skid") != NULL;
+
+ if (skid_steering) {
+ // these are taken from a 6V wild thumper with skid steering,
+ // with a sabertooth controller
+ max_accel = 14;
+ max_speed = 4;
+ }
+}
+
+
+
+/*
+ return turning circle (diameter) in meters for steering angle proportion in degrees
+*/
+float Rover::turn_circle(float steering)
+{
+ if (fabsf(steering) < 1.0e-6) {
+ return 0;
+ }
+ return turning_circle * sinf(radians(35)) / sinf(radians(steering*35));
+}
+
+/*
+ return yaw rate in degrees/second given steering_angle and speed
+*/
+float Rover::calc_yaw_rate(float steering, float speed)
+{
+ if (skid_steering) {
+ return steering * skid_turn_rate;
+ }
+ if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) {
+ return 0;
+ }
+ float d = turn_circle(steering);
+ float c = M_PI_F * d;
+ float t = c / speed;
+ float rate = 360.0f / t;
+ return rate;
+}
+
+/*
+ return lateral acceleration in m/s/s
+*/
+float Rover::calc_lat_accel(float steering_angle, float speed)
+{
+ float yaw_rate = calc_yaw_rate(steering_angle, speed);
+ float accel = radians(yaw_rate) * speed;
+ return accel;
+}
+
+/*
+ update the rover simulation by one time step
+ */
+void Rover::update(const struct sitl_input &input)
+{
+ float steering, throttle;
+
+ // if in skid steering mode the steering and throttle values are used for motor1 and motor2
+ if (skid_steering) {
+ float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+ float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+ steering = motor1 - motor2;
+ throttle = 0.5*(motor1 + motor2);
+ } else {
+ steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+ throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+ }
+
+ // how much time has passed?
+ float delta_time = frame_time_us * 1.0e-6f;
+
+ // speed in m/s in body frame
+ Vector3f velocity_body = dcm.transposed() * velocity_ef;
+
+ // speed along x axis, +ve is forward
+ float speed = velocity_body.x;
+
+ // yaw rate in degrees/s
+ float yaw_rate = calc_yaw_rate(steering, speed);
+
+ // target speed with current throttle
+ float target_speed = throttle * max_speed;
+
+ // linear acceleration in m/s/s - very crude model
+ float accel = max_accel * (target_speed - speed) / max_speed;
+
+ gyro = Vector3f(0,0,radians(yaw_rate));
+
+ // update attitude
+ dcm.rotate(gyro * delta_time);
+ dcm.normalize();
+
+ // accel in body frame due to motor
+ accel_body = Vector3f(accel, 0, 0);
+
+ // add in accel due to direction change
+ accel_body.y += radians(yaw_rate) * speed;
+
+ // now in earth frame
+ Vector3f accel_earth = dcm * accel_body;
+ accel_earth += Vector3f(0, 0, GRAVITY_MSS);
+
+ // we are on the ground, so our vertical accel is zero
+ accel_earth.z = 0;
+
+ // work out acceleration as seen by the accelerometers. It sees the kinematic
+ // acceleration (ie. real movement), plus gravity
+ accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
+
+ // new velocity vector
+ velocity_ef += accel_earth * delta_time;
+
+ // new position vector
+ position += velocity_ef * delta_time;
+ position.z = -home.alt*0.01f;
+
+ // update lat/lon/altitude
+ update_position();
+}
diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h
new file mode 100644
index 0000000000..c1026b806b
--- /dev/null
+++ b/libraries/SITL/SIM_Rover.h
@@ -0,0 +1,55 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ rover simulator class
+*/
+
+#ifndef _SIM_ROVER_H
+#define _SIM_ROVER_H
+
+#include "SIM_Aircraft.h"
+
+/*
+ a rover simulator
+ */
+class Rover : public Aircraft
+{
+public:
+ Rover(const char *home_str, const char *frame_str);
+
+ /* update model by one time step */
+ void update(const struct sitl_input &input);
+
+ /* static object creator */
+ static Aircraft *create(const char *home_str, const char *frame_str) { return new Rover(home_str, frame_str); }
+
+private:
+ float max_speed;
+ float max_accel;
+ float wheelbase;
+ float wheeltrack;
+ float max_wheel_turn;
+ float turning_circle;
+ float skid_turn_rate;
+ bool skid_steering;
+
+ float turn_circle(float steering);
+ float calc_yaw_rate(float steering, float speed);
+ float calc_lat_accel(float steering_angle, float speed);
+};
+
+
+#endif // _SIM_ROVER_H