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.
99 lines
3.3 KiB
99 lines
3.3 KiB
#!/usr/bin/env python |
|
''' |
|
simple antenna tracker simulator core |
|
''' |
|
|
|
from aircraft import Aircraft |
|
import util, time, math |
|
from math import degrees, radians |
|
from rotmat import Vector3 |
|
|
|
class Tracker(Aircraft): |
|
'''a simple antenna tracker''' |
|
def __init__(self, |
|
rate_controlled=False, |
|
pitch_range = 45, |
|
yaw_range = 180, |
|
zero_yaw = 270, # yaw direction at startup |
|
zero_pitch = 10, # pitch at startup |
|
turn_rate=90 # servo max turn rate in degrees/sec |
|
): |
|
Aircraft.__init__(self) |
|
self.rate_controlled = rate_controlled |
|
self.turn_rate = turn_rate |
|
self.last_time = time.time() |
|
self.pitch_range = pitch_range |
|
self.yaw_range = yaw_range |
|
self.zero_yaw = zero_yaw |
|
self.zero_pitch = zero_pitch |
|
self.verbose = False |
|
self.last_debug = time.time() |
|
self.pitch_current = 0 |
|
self.yaw_current = 0 |
|
|
|
def slew_limit(self, current, target, range, delta_time): |
|
'''limit speed of servo movement''' |
|
dangle = self.turn_rate * delta_time |
|
dv = dangle / range |
|
if target - current > dv: |
|
return current + dv |
|
if target - current < -dv: |
|
return current - dv |
|
return target |
|
|
|
|
|
def update(self, state): |
|
# how much time has passed? |
|
t = time.time() |
|
delta_time = t - self.last_time |
|
self.last_time = t |
|
|
|
self.pitch_current = self.slew_limit(self.pitch_current, state.pitch_input, self.pitch_range, delta_time) |
|
self.yaw_current = self.slew_limit(self.yaw_current, state.yaw_input, self.yaw_range, delta_time) |
|
|
|
pitch_target = self.zero_pitch + self.pitch_current*self.pitch_range |
|
yaw_target = self.zero_yaw + self.yaw_current*self.yaw_range |
|
while yaw_target > 180: |
|
yaw_target -= 360 |
|
|
|
(r,p,y) = self.dcm.to_euler() |
|
pitch_current = degrees(p) |
|
yaw_current = degrees(y) |
|
roll_current = degrees(r) |
|
|
|
pitch_rate = pitch_target - pitch_current |
|
pitch_rate = min(self.turn_rate, pitch_rate) |
|
pitch_rate = max(-self.turn_rate, pitch_rate) |
|
|
|
yaw_diff = yaw_target - yaw_current |
|
if yaw_diff > 180: |
|
yaw_diff -= 360 |
|
if yaw_diff < -180: |
|
yaw_diff += 360 |
|
yaw_rate = yaw_diff |
|
yaw_rate = min(self.turn_rate, yaw_rate) |
|
yaw_rate = max(-self.turn_rate, yaw_rate) |
|
|
|
# keep it level |
|
roll_rate = 0 - roll_current |
|
|
|
if time.time() - self.last_debug > 2: |
|
self.last_debug = time.time() |
|
print("roll=%.1f/%.1f pitch=%.1f/%.1f yaw=%.1f/%.1f rates=%.1f/%.1f/%.1f" % ( |
|
roll_current, 0, |
|
pitch_current, pitch_target, |
|
yaw_current, yaw_target, |
|
roll_rate, pitch_rate, yaw_rate)) |
|
|
|
self.gyro = Vector3(radians(roll_rate),radians(pitch_rate),radians(yaw_rate)) |
|
|
|
# update attitude |
|
self.dcm.rotate(self.gyro * delta_time) |
|
self.dcm.normalize() |
|
|
|
accel_earth = Vector3(0, 0, -self.gravity) |
|
self.accel_body = self.dcm.transposed() * accel_earth |
|
|
|
# new velocity vector |
|
self.velocity = Vector3() |
|
self.update_position(delta_time)
|
|
|