Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : rover simulator class
17 : */
18 :
19 : #include "SIM_Rover.h"
20 :
21 : #include <string.h>
22 : #include <stdio.h>
23 :
24 : #include <AP_Math/AP_Math.h>
25 :
26 : namespace SITL {
27 :
28 245 : SimRover::SimRover(const char *frame_str) :
29 245 : Aircraft(frame_str)
30 : {
31 245 : constexpr float default_battery_resistance_ohm = 0.01f;
32 490 : battery.setup(sitl->batt_capacity_ah,
33 : default_battery_resistance_ohm,
34 245 : sitl->batt_voltage,
35 : ambient_outside_temperature_degC());
36 245 : battery_voltage = battery.get_voltage();
37 :
38 245 : skid_steering = strstr(frame_str, "skid") != nullptr;
39 :
40 245 : if (skid_steering) {
41 2 : printf("SKID Steering Rover Simulation Started\n");
42 : // these are taken from a 6V wild thumper with skid steering,
43 : // with a sabertooth controller
44 2 : max_accel = 14;
45 2 : max_speed = 4;
46 2 : return;
47 : }
48 :
49 243 : vectored_thrust = strstr(frame_str, "vector") != nullptr;
50 243 : if (vectored_thrust) {
51 1 : printf("Vectored Thrust Rover Simulation Started\n");
52 : }
53 :
54 243 : omni3 = strstr(frame_str, "omni3mecanum") != nullptr;
55 243 : if (omni3) {
56 1 : printf("Omni3 Mecanum Rover Simulation Started\n");
57 : }
58 :
59 243 : lock_step_scheduled = true;
60 : }
61 :
62 : /*
63 : return turning circle (diameter) in meters for steering angle proportion in degrees
64 : */
65 2363364 : float SimRover::turn_circle(float steering) const
66 : {
67 2363364 : if (fabsf(steering) < 1.0e-6) {
68 0 : return 0;
69 : }
70 2363364 : return turning_circle * sinf(radians(max_wheel_turn)) / sinf(radians(steering*max_wheel_turn));
71 : }
72 :
73 : /*
74 : return yaw rate in degrees/second given steering_angle and speed
75 : */
76 13142737 : float SimRover::calc_yaw_rate(float steering, float speed)
77 : {
78 13142737 : if (skid_steering) {
79 206934 : return constrain_float(steering * skid_turn_rate, -MAX_YAW_RATE, MAX_YAW_RATE);
80 : }
81 12935803 : if (vectored_thrust) {
82 117999 : return constrain_float(steering * vectored_turn_rate_max, -MAX_YAW_RATE, MAX_YAW_RATE);
83 : }
84 12817804 : if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) {
85 10454440 : return 0;
86 : }
87 2363364 : float d = turn_circle(steering);
88 2363364 : float c = M_PI * d;
89 2363364 : float t = c / speed;
90 2363364 : float rate = constrain_float(360.0f / t, -MAX_YAW_RATE, MAX_YAW_RATE);
91 2363364 : return rate;
92 : }
93 :
94 : /*
95 : return lateral acceleration in m/s/s
96 : */
97 0 : float SimRover::calc_lat_accel(float steering_angle, float speed)
98 : {
99 0 : float yaw_rate = calc_yaw_rate(steering_angle, speed);
100 0 : float accel = radians(yaw_rate) * speed;
101 0 : return accel;
102 : }
103 :
104 : /*
105 : update the rover simulation by one time step
106 : */
107 13407122 : void SimRover::update(const struct sitl_input &input)
108 : {
109 : // how much time has passed?
110 13407122 : float delta_time = frame_time_us * 1.0e-6f;
111 :
112 : // update gyro and accel_body according to frame type
113 13407122 : if (omni3) {
114 264385 : update_omni3(input, delta_time);
115 : } else {
116 13142737 : update_ackermann_or_skid(input, delta_time);
117 : }
118 :
119 : // common to all rovers
120 :
121 : // now in earth frame
122 13407122 : Vector3f accel_earth = dcm * accel_body;
123 13407122 : accel_earth += Vector3f(0, 0, GRAVITY_MSS);
124 :
125 : // we are on the ground, so our vertical accel is zero
126 13407122 : accel_earth.z = 0;
127 :
128 : // work out acceleration as seen by the accelerometers. It sees the kinematic
129 : // acceleration (ie. real movement), plus gravity
130 13407122 : accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
131 :
132 : // new velocity vector
133 13407122 : velocity_ef += accel_earth * delta_time;
134 :
135 : // new position vector
136 13407122 : position += (velocity_ef * delta_time).todouble();
137 :
138 13407122 : update_external_payload(input);
139 :
140 : // update lat/lon/altitude
141 13407122 : update_position();
142 13407122 : time_advance();
143 :
144 : // update magnetic field
145 13407122 : update_mag_field_bf();
146 :
147 13407122 : update_battery();
148 13407122 : }
149 :
150 : /*
151 : update the ackermann or skid rover simulation by one time step
152 : */
153 13142737 : void SimRover::update_ackermann_or_skid(const struct sitl_input &input, float delta_time)
154 : {
155 13142737 : float steering = 0.0f;
156 13142737 : float throttle = 0.0f;
157 :
158 13142737 : if (!battery_is_empty()) {
159 : // if in skid steering mode the steering and throttle values are used for motor1 and motor2
160 13142737 : if (skid_steering) {
161 206934 : const float motor1 = input.servos[0] ? normalise_servo_input(input.servos[0]) : 0;
162 206934 : const float motor2 = input.servos[2] ? normalise_servo_input(input.servos[2]) : 0;
163 206934 : steering = motor1 - motor2;
164 206934 : throttle = 0.5*(motor1 + motor2);
165 206934 : battery_current = 10.0f * (fabsf(motor1) + fabsf(motor2));
166 : } else {
167 : // steering here should really be "old steering" as no-pulses
168 : // should yield no servo movement
169 12935803 : steering = input.servos[0] ? normalise_servo_input(input.servos[0]) : 0;
170 12935803 : throttle = input.servos[2] ? normalise_servo_input(input.servos[2]) : 0;
171 12935803 : battery_current = 20.0f * fabsf(throttle) + 1.0f * fabsf(steering);
172 :
173 : // vectored thrust conversion
174 12935803 : if (vectored_thrust) {
175 117999 : const float thrust = throttle;
176 117999 : const float steering_angle_rad = radians(steering * vectored_angle_max);
177 117999 : steering = sinf(steering_angle_rad) * thrust;
178 117999 : throttle = cosf(steering_angle_rad) * thrust;
179 117999 : battery_current = 20.0f * fabsf(thrust);
180 : }
181 : }
182 : } else {
183 0 : battery_current = 0.0f;
184 : }
185 :
186 : // speed in m/s in body frame
187 13142737 : Vector3f velocity_body = dcm.transposed() * velocity_ef;
188 :
189 : // speed along x axis, +ve is forward
190 13142737 : float speed = velocity_body.x;
191 :
192 : // yaw rate in degrees/s
193 13142737 : float yaw_rate = calc_yaw_rate(steering, speed);
194 :
195 : // target speed with current throttle
196 13142737 : float target_speed = throttle * max_speed;
197 :
198 : // linear acceleration in m/s/s - very crude model
199 13142737 : float accel = max_accel * (target_speed - speed) / max_speed;
200 :
201 13142737 : gyro = Vector3f(0,0,radians(yaw_rate));
202 :
203 : // update attitude
204 13142737 : dcm.rotate(gyro * delta_time);
205 13142737 : dcm.normalize();
206 :
207 : // accel in body frame due to motor (excluding gravity)
208 13142737 : accel_body = Vector3f(accel, 0, 0);
209 :
210 : // add in accel due to direction change
211 13142737 : accel_body.y += radians(yaw_rate) * speed;
212 13142737 : }
213 :
214 : /*
215 : update the omni3 rover simulation by one time step
216 : */
217 264385 : void SimRover::update_omni3(const struct sitl_input &input, float delta_time)
218 : {
219 : // in omni3 mode the first three servos are motor speeds
220 :
221 : // use forward kinematics to calculate body frame velocity
222 264385 : Vector3f wheel_ang_vel;
223 264385 : battery_current = 0.0f;
224 264385 : if (!battery_is_empty()) {
225 1057540 : for (uint8_t i=0; i<3; i++) {
226 793155 : wheel_ang_vel[i] = input.servos[i] ? normalise_servo_input(input.servos[i]) : 0;
227 793155 : battery_current += 10.0f * fabsf(wheel_ang_vel[i]);
228 : };
229 264385 : wheel_ang_vel *= omni3_wheel_max_ang_vel;
230 : }
231 :
232 : // derivation of forward kinematics for an Omni3Mecanum rover
233 : // A. Gfrerrer. "Geometry and kinematics of the Mecanum wheel",
234 : // Computer Aided Geometric Design 25 (2008) 784–791.
235 : // Retrieved from https://www.geometrie.tugraz.at/gfrerrer/publications/MecanumWheel.pdf.
236 : //
237 : // the frame is equilateral triangle
238 : //
239 : // d[i] = 0.18 m is distance from frame centre to each wheel
240 : // r_w = 0.04725 m is the wheel radius.
241 : // delta = radians(-45) is angle of the roller to the direction of forward rotation
242 : // alpha[i] is the angle the wheel axis is rotated about the body z-axis
243 : // c[i] = cos(alpha[i] + delta)
244 : // s[i] = sin(alpha[i] + delta)
245 : //
246 : // wheel d[i] alpha[i] a_x[i] a_y[i] c[i] s[i]
247 : // 1 0.18 1.04719 0.09 0.15588 0.965925 0.258819
248 : // 2 0.18 3.14159 -0.18 0.0 -0.707106 0.707106
249 : // 3 0.18 5.23598 0.09 -0.15588 -0.258819 -0.965925
250 : //
251 : // k = 1/(r_w * sin(delta)) = -29.930445 is a scale factor
252 : //
253 : // inverse kinematic matrix
254 : // M[i, 0] = k * c[i]
255 : // M[i, 1] = k * s[i]
256 : // M[i, 2] = k * (a_x[i] s[i] - a_y[i] c[i])
257 : //
258 : // forward kinematics matrix: Minv = M^-1
259 264385 : constexpr Matrix3f Minv(
260 : -0.0215149, 0.01575, 0.0057649,
261 : -0.0057649, -0.01575, 0.0215149,
262 : 0.0875, 0.0875, 0.0875);
263 :
264 : // twist - this is the target linear and angular velocity
265 264385 : Vector3f twist = Minv * wheel_ang_vel;
266 :
267 : // speed in m/s in body frame
268 264385 : Vector3f velocity_body = dcm.transposed() * velocity_ef;
269 :
270 : // linear acceleration in m/s/s - very crude model
271 264385 : float accel_x = omni3_max_accel * (twist.x - velocity_body.x) / omni3_max_speed;
272 264385 : float accel_y = omni3_max_accel * (twist.y - velocity_body.y) / omni3_max_speed;
273 :
274 264385 : gyro = Vector3f(0, 0, twist.z);
275 :
276 : // update attitude
277 264385 : dcm.rotate(gyro * delta_time);
278 264385 : dcm.normalize();
279 :
280 : // accel in body frame due to motors (excluding gravity)
281 264385 : accel_body = Vector3f(accel_x, accel_y, 0);
282 264385 : }
283 :
284 13407122 : void SimRover::update_battery()
285 : {
286 13407122 : battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
287 13407122 : const uint64_t now_us = AP_HAL::micros64();
288 13407122 : battery.consume_energy(battery_current, now_us);
289 13407122 : battery_voltage = battery.get_voltage();
290 13407122 : battery_temperature_degC = battery.get_temperature_degC();
291 13407122 : }
292 :
293 : } // namespace SITL
|
