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