# 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())