You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
78 lines
2.5 KiB
78 lines
2.5 KiB
#!/usr/bin/env python |
|
''' |
|
simple rover simulator core |
|
''' |
|
|
|
from aircraft import Aircraft |
|
import util, time, math |
|
from math import degrees, radians |
|
from rotmat import Vector3, Matrix3 |
|
|
|
class Rover(Aircraft): |
|
'''a simple rover''' |
|
def __init__(self, |
|
max_speed=10, |
|
max_accel=10, |
|
max_turn_rate=45): |
|
Aircraft.__init__(self) |
|
self.max_speed = max_speed |
|
self.max_accel = max_accel |
|
self.max_turn_rate = max_turn_rate |
|
self.last_time = time.time() |
|
|
|
def update(self, state): |
|
# how much time has passed? |
|
t = time.time() |
|
delta_time = t - self.last_time |
|
self.last_time = t |
|
|
|
# speed in m/s in body frame |
|
velocity_body = self.dcm.transposed() * self.velocity |
|
|
|
# speed along x axis, +ve is forward |
|
speed = velocity_body.x |
|
|
|
# yaw rate in degrees/s |
|
yaw_rate = self.max_turn_rate * state.steering * (speed / self.max_speed) |
|
|
|
# target speed with current throttle |
|
target_speed = state.throttle * self.max_speed |
|
|
|
# linear acceleration in m/s/s - very crude model |
|
accel = self.max_accel * (target_speed - speed) / self.max_speed |
|
|
|
# print('speed=%f throttle=%f steering=%f yaw_rate=%f accel=%f' % (speed, state.throttle, state.steering, yaw_rate, accel)) |
|
|
|
self.gyro = Vector3(0,0,radians(yaw_rate)) |
|
|
|
# update attitude |
|
self.dcm.rotate(self.gyro * delta_time) |
|
self.dcm.normalize() |
|
|
|
# accel in body frame due to motor |
|
accel_body = Vector3(accel, 0, 0) |
|
|
|
# add in accel due to direction change |
|
accel_body.y += radians(yaw_rate) * speed |
|
|
|
# now in earth frame |
|
accel_earth = self.dcm * accel_body |
|
accel_earth += Vector3(0, 0, self.gravity) |
|
|
|
# if we're on the ground, then our vertical acceleration is limited |
|
# to zero. This effectively adds the force of the ground on the aircraft |
|
accel_earth.z = 0 |
|
|
|
# work out acceleration as seen by the accelerometers. It sees the kinematic |
|
# acceleration (ie. real movement), plus gravity |
|
self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity)) |
|
|
|
# new velocity vector |
|
self.velocity += accel_earth * delta_time |
|
|
|
# new position vector |
|
old_position = self.position.copy() |
|
self.position += self.velocity * delta_time |
|
|
|
# update lat/lon/altitude |
|
self.update_position(delta_time)
|
|
|