# Add some Guassian noise to position
positionXMeters = random.gauss(positionXMeters, GPS_NOISE_METERS)
positionYMeters = random.gauss(positionYMeters, GPS_NOISE_METERS)
# Convert Euler angles to pitch, roll, yaw
# See http://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft) for positive/negative orientation
rollAngleRadians, pitchAngleRadians = rotate((alphaRadians, betaRadians), gammaRadians)
pitchAngleRadians = -pitchAngleRadians
yawAngleRadians = -gammaRadians
# Get altitude directly from position Z
altitudeMeters = positionZMeters
# Poll controller
demands = controller.poll()
# Get motor thrusts from FMU model
thrusts = fmu.getMotors((pitchAngleRadians, rollAngleRadians, yawAngleRadians), altitudeMeters, \
coordcalc.metersToDegrees(positionXMeters, positionYMeters),\
demands, timestepSeconds)
# Force is a function of particle count
particleCount = int(particleCountPerSecond * timestepSeconds)
# Compute force and torque for each propeller
forcesAndTorques = [0]*24
for i in range(4):
force = particleCount * particleDensity * thrusts[i] * pi * particleSizes[i]**3 / (6*timestepSeconds)
torque = ((-1)**(i+1))*.002 * thrusts[i]
'''
qstest.py - PyQuadStick test program
Copyright (C) 2014 Simon D. Levy
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser 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.
'''
# Choose your controller ======================================================
#from quadstick import ExtremePro3D as Controller
from quadstick import PS3 as Controller
#from quadstick.rc.spektrum import DX8 as Controller
#from quadstick.rc.frsky import Taranis as Controller
# =============================================================================
controller = Controller(('0', '1', '2'))
while controller.running():
print('%+3.3f %+3.3f %+3.3f %+3.3f %d' % controller.poll())
# Get core data from client
coreData = receiveFloats(client, 4)
# Quit on timeout
if not coreData: exit(0)
# Get extra data from client
extraData = getAdditionalData(client, receiveFloats)
# Unpack IMU data
timestep = coreData[0] # seconds
pitch = coreData[1] # positive = nose up
roll = coreData[2] # positive = right down
yaw = coreData[3] # positive = nose right
# Poll controller
demands = controller.poll()
# Get motor thrusts from quadrotor model
thrusts = fmu.getMotors((pitch, roll, yaw), demands, timestep,
extraData)
# Send thrusts to client
sendFloats(client, thrusts)
except Exception:
# Inform and exit on exception
controller.error()
exit(0)
qstest.py - PyQuadStick test program
Copyright (C) 2014 Simon D. Levy
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser 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.
'''
# Choose your controller ======================================================
#from quadstick import ExtremePro3D as Controller
from quadstick import PS3 as Controller
#from quadstick.rc.spektrum import DX8 as Controller
#from quadstick.rc.frsky import Taranis as Controller
# =============================================================================
controller = Controller(('0', '1', '2'))
while controller.running():
print('%+3.3f %+3.3f %+3.3f %+3.3f %d' % controller.poll())
