class VisualFlightController(object):
PID_ANGLES_SPEED_PERIOD = 0.05 #seconds
PID_ANGLES_SPEED_KP = 0.0
PID_ANGLES_SPEED_KI = 0.02
PID_ANGLES_SPEED_KD = 0.08
_instance = None
@staticmethod
def getInstance():
if VisualFlightController._instance == None:
VisualFlightController._instance = VisualFlightController()
return VisualFlightController._instance
def __init__(self):
self._driver = Driver()
self._sensor = VisualTracker()
kpMatrix = [VisualFlightController.PID_ANGLES_SPEED_KP]*4
kiMatrix = [VisualFlightController.PID_ANGLES_SPEED_KI]*4
kdMatrix = [VisualFlightController.PID_ANGLES_SPEED_KD]*4
tolerances = [20.0, 20.0, 20.0, 10.0]
initialTargets = [0.0]*4
self._pidAnglesSpeed = PID(VisualFlightController.PID_ANGLES_SPEED_PERIOD, kpMatrix, kiMatrix, kdMatrix, \
self._readSensor, self._setResult, tolerances, len(kpMatrix))
self._pidAnglesSpeed.setTargets(initialTargets)
self._isRunning = False
def _readSensor(self):
inputData = self._sensor.track()
inputData[2] = 0.0 #Z-input is disabled
return inputData
def _setX(self, increment):
if increment != 0.0:
self._driver.shiftX(increment)
def _setY(self, increment):
if increment != 0.0:
self._driver.shiftY(increment)
def _setZ(self, increment):
if increment != 0.0:
self._driver.addThrottle(increment)
def _setAZ(self, increment):
if increment != 0.0:
self._driver.spin(increment)
def _setResult(self, results):
self._setX(results[0])
self._setY(results[1])
#self._setZ(results[2])
self._setAZ(results[3])
def addThrottle(self, increment):
self._driver.addThrottle(increment)
'''
def shift(self, angle, radius):
acc = VisualFlightController.MAX_ACCELERATION * radius / 100.0
xacc = sin(angle) * acc
yacc = cos(angle) * acc
targetX = VisualFlightController._g2SensorUnit(xacc)
targetY = VisualFlightController._g2SensorUnit(yacc)
self._pidX.setTarget(targetX, 0)
self._pidY.setTarget(targetY, 1)
#logging.debug("Target: {0}".format(self._accTarget))
'''
def start(self):
self._isRunning = True
#self._sensor.start()
self._driver.start()
self._driver.standBy()
self._pidAnglesSpeed.start()
def stop(self):
self._isRunning = False
self._pidAnglesSpeed.stop()
self.standBy()
self._driver.stop()
self._sensor.stop()
def standBy(self):
self._driver.standBy()
def idle(self):
self._driver.idle()
class FlightController(object):
#20160129 DPM - sleep function has upto millesecond precision
PID_PERIOD = 0.02 # seconds (~50Hz)
_instance = None
@staticmethod
def getInstance():
if FlightController._instance == None:
FlightController._instance = FlightController()
return FlightController._instance
def __init__(self):
self._config = Configuration.getInstance().getConfig()
self._driver = Driver()
self._createSensor(self._config[Configuration.KEY_IMU_CLASS])
#PID constants must have the same length
self._pidAnglesSpeedKP = self._config[Configuration.PID_ANGLES_SPEED_KP]
self._pidAnglesSpeedKI = self._config[Configuration.PID_ANGLES_SPEED_KI]
self._pidAnglesSpeedKD = self._config[Configuration.PID_ANGLES_SPEED_KD]
#PID constants must have the same length
self._pidAnglesKP = self._config[Configuration.PID_ANGLES_KP]
self._pidAnglesKI = self._config[Configuration.PID_ANGLES_KI]
self._pidAnglesKD = self._config[Configuration.PID_ANGLES_KD]
self._pidAccelKP = self._config[Configuration.PID_ACCEL_KP]
self._pidAccelKI = self._config[Configuration.PID_ACCEL_KI]
self._pidAccelKD = self._config[Configuration.PID_ACCEL_KD]
#PID
self._pidKP = self._pidAnglesSpeedKP + self._pidAnglesKP + self._pidAccelKP
self._pidKI = self._pidAnglesSpeedKI + self._pidAnglesKI + self._pidAccelKI
self._pidKD = self._pidAnglesSpeedKD + self._pidAnglesKD + self._pidAccelKD
self._pid = PID(FlightController.PID_PERIOD, \
self._pidKP, self._pidKI, self._pidKD, \
self._readPIDInput, self._setPIDOutput, \
"stabilization-pid")
self._pid.setTargets([0.0]*len(self._pidKP))
self._isRunning = False
def _createSensor(self, imuClass):
if imuClass == Configuration.VALUE_IMU_CLASS_3000:
self._sensor = Sensor()
elif imuClass == Configuration.VALUE_IMU_CLASS_REMOTE:
self._remote = rpyc.classic.connect(self._config[Configuration.KEY_REMOTE_ADDRESS])
imuDummy = self._remote.modules["sensors.IMU_dummy"]
self._sensor = imuDummy.IMUDummy()
elif imuClass == Configuration.VALUE_IMU_CLASS_3000EMU:
self._sensor = Imu3000Emulated()
elif imuClass == Configuration.VALUE_IMU_CLASS_6050:
self._sensor = Imu6050()
elif imuClass == Configuration.VALUE_IMU_CLASS_EMULATION:
self._sensor = EmulatedSensor()
else: #Dummy as default
self._sensor = IMUDummy()
def _readPIDAnglesSpeedInput(self):
inputData = self._sensor.readAngleSpeeds()
logging.debug("PID-angles-speed input: {0}".format(inputData))
return inputData
def _setPIDAnglesSpeedOutput(self, output):
#angle X
#Moving the drone along Y-direction makes it turning on X-axis
#Caution! Angle around X-axis as positive sense means moving backwards
self._driver.shiftY(-output[0])
#angle Y
#Moving the drone along X-direction makes it turning on Y-axis
self._driver.shiftX(output[1])
#angle Z
self._driver.spin(output[2])
logging.debug("PID-angles-speed output: {0}".format(output))
def _readPIDAnglesInput(self):
angles = self._sensor.readDeviceAngles()
logging.debug("PID-angles input: {0}".format(angles))
return angles
def _setPIDAnglesOutput(self, output):
#angle X
self._pid.setTarget(output[0], 0)
#angle Y
self._pid.setTarget(output[1], 1)
logging.debug("PID-angles output: {0}".format(output))
def _readPIDAccelInput(self):
inputData = self._sensor.readAccels()
logging.debug("PID-accel input: {0}".format(inputData))
return inputData
def _setPIDAccelOutput(self, output):
#accel Z
self._driver.addThrottle(output[2])
logging.debug("PID-accel output: {0}".format(output))
def _readPIDInput(self):
logging.debug("Targets: {0}".format(self._pid.getTargets()))
self._sensor.refreshState()
anglesSpeedInput = self._readPIDAnglesSpeedInput()
anglesInput = self._readPIDAnglesInput()[0:2]
accelInput = self._readPIDAccelInput()
pidInput = anglesSpeedInput + anglesInput + accelInput
return pidInput
def _setPIDOutput(self, output):
anglesSpeedOutput = output[0:3]
anglesOutput = output[3:5]
accelOutput = output[5:8]
self._setPIDAnglesSpeedOutput(anglesSpeedOutput)
self._setPIDAnglesOutput(anglesOutput)
self._setPIDAccelOutput(accelOutput)
self._driver.commitIncrements()
def addThrottle(self, increment):
self._driver.addThrottle(increment)
def setTargets(self, targets):
self._pid.setTarget(targets[0], 3) #angle X
self._pid.setTarget(targets[1], 4) #angle Y
self._pid.setTarget(targets[2], 2) #angle speed Z
self._pid.setTarget(targets[3], 7) #accel Z
def enableIntegrals(self):
self._pid.enableIntegrals()
def disableIntegrals(self):
self._pid.disableIntegrals()
def start(self):
if not self._isRunning:
self._isRunning = True
self._sensor.start()
self._driver.start()
self._driver.idle()
def startPid(self):
if self._isRunning:
print "Starting PID..."
logging.info("Starting PID...")
self._sensor.resetGyroReadTime()
time.sleep(FlightController.PID_PERIOD)
self._sensor.refreshState()
Thread(target=self._doFlightDetection).start()
self._pid.start()
def stopPid(self):
self._pid.stop()
print "PID finished."
logging.info("PID finished.")
def stop(self):
if self._isRunning:
self._isRunning = False
self.stopPid()
self.idle()
self._driver.stop()
self._sensor.stop()
def standBy(self):
self._driver.standBy()
def idle(self):
self._driver.idle()
def alterPidAnglesSpeedConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesSpeedKP):
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
def alterPidAnglesConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesKP):
axisIndex += 3
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
def alterPidAccelConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAccelKP):
axisIndex += 5
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
def isRunning(self):
return self._isRunning
def readState(self):
state = State()
state._throttles = self._driver.getThrottles()
state._angles = self._sensor.readDeviceAngles()
state._angles[2] = self._sensor.readAngleSpeeds()[2]
state._accels = self._sensor.readAccels()
return state
def _doFlightDetection(self):
for index in range(3):
self.alterPidAnglesSpeedConstants(index, 0.0, 0.0, 0.0)
for index in range(2):
self.alterPidAnglesConstants(index, 0.0, 0.0, 0.0)
self._pid.disableIntegrals()
maxError = self._sensor.getMaxErrorZ()
accelZ = self._sensor.readAccels()[2]
while self._isRunning and accelZ < maxError:
accelZ = self._sensor.readAccels()[2]
time.sleep(FlightController.PID_PERIOD)
if self._isRunning:
message="Flight detected: accel-Z = {0:.3f}".format(accelZ)
logging.debug(message)
print message
for index in range(3):
self.alterPidAnglesSpeedConstants(index, self._pidAnglesSpeedKP[index], self._pidAnglesSpeedKI[index], self._pidAnglesSpeedKD[index])
for index in range(2):
self.alterPidAnglesConstants(index, self._pidAnglesKP[index], self._pidAnglesKI[index], self._pidAnglesKD[index])
self._pid.enableIntegrals()
class FlightController(object):
PID_PERIOD = 0.006 #166.666Hz #0.02 # seconds (50Hz)
ANGLE_SPEED_FACTOR = 1000.0 # Angle-speed's PID-constants are divided by this factor
FLIGHT_MODE_ANGLE_SPEED = 0
FLIGHT_MODE_ANGLE = 1
FLIGHT_MODE_ACCEL = 2
_instance = None
@staticmethod
def getInstance():
if FlightController._instance == None:
FlightController._instance = FlightController()
return FlightController._instance
def __init__(self):
self._config = Configuration.getInstance().getConfig()
self._driver = Driver()
self._createSensor(self._config[Configuration.KEY_IMU_CLASS])
#PID constants must have the same length
self._pidAnglesSpeedKP = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KP]]
self._pidAnglesSpeedKI = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KI]]
self._pidAnglesSpeedKD = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KD]]
#PID constants must have the same length
self._pidAnglesKP = self._config[Configuration.PID_ANGLES_KP]
self._pidAnglesKI = self._config[Configuration.PID_ANGLES_KI]
self._pidAnglesKD = self._config[Configuration.PID_ANGLES_KD]
self._pidAccelKP = self._config[Configuration.PID_ACCEL_KP]
self._pidAccelKI = self._config[Configuration.PID_ACCEL_KI]
self._pidAccelKD = self._config[Configuration.PID_ACCEL_KD]
#PID
self._pidKP = self._pidAnglesSpeedKP + self._pidAnglesKP + self._pidAccelKP
self._pidKI = self._pidAnglesSpeedKI + self._pidAnglesKI + self._pidAccelKI
self._pidKD = self._pidAnglesSpeedKD + self._pidAnglesKD + self._pidAccelKD
self._pid = PID(FlightController.PID_PERIOD, \
self._pidKP, self._pidKI, self._pidKD, \
self._readPIDInput, self._setPIDOutput, \
"stabilization-pid")
self._pid.setTargets([0.0] * len(self._pidKP))
self._isRunning = False
self._pidThrottleThreshold = 0
self._maxAngleX = self._config[Configuration.KEY_MAX_ANGLE_X]
self._maxAngleY = self._config[Configuration.KEY_MAX_ANGLE_Y]
#TODO: Rate mode
#self._maxAngleSpeedX = self._config[Configuration.KEY_MAX_ANGLE_SPEED_X] #used for rate mode when implemented
#self._maxAngleSpeedY = self._config[Configuration.KEY_MAX_ANGLE_SPEED_Y] #used for rate mode when implemented
self._maxAngleSpeedZ = self._config[
Configuration.KEY_MAX_ANGLE_SPEED_Z]
#TODO: Read initial flight mode from configuration
self._flightMode = FlightController.FLIGHT_MODE_ANGLE
def setPidThrottleThreshold(self, throttle):
self._pidThrottleThreshold = throttle
def _createSensor(self, imuClass):
if imuClass == Configuration.VALUE_IMU_CLASS_3000:
self._sensor = Sensor()
# elif imuClass == Configuration.VALUE_IMU_CLASS_REMOTE:
#
# self._remote = rpyc.classic.connect(self._config[Configuration.KEY_REMOTE_ADDRESS])
# imuDummy = self._remote.modules["sensors.IMU_dummy"]
# self._sensor = imuDummy.IMUDummy()
elif imuClass == Configuration.VALUE_IMU_CLASS_3000EMU:
self._sensor = Imu3000Emulated()
elif imuClass == Configuration.VALUE_IMU_CLASS_6050:
self._sensor = Imu6050Dmp()
elif imuClass == Configuration.VALUE_IMU_CLASS_EMULATION:
self._sensor = EmulatedSensor(EmulatedDrone.REALISTIC_FLIGHT)
else: #Dummy as default
self._sensor = IMUDummy()
def _readPIDAnglesSpeedInput(self):
inputData = self._sensor.readAngleSpeeds()
logging.debug("PID-angles-speed input: {0}".format(inputData))
return inputData
def _setPIDAnglesSpeedOutput(self, output):
#angle X
#Moving the drone along Y-direction makes it turning on X-axis
#Caution! Angle around X-axis as positive sense means moving backwards
self._driver.shiftY(-output[0])
#angle Y
#Moving the drone along X-direction makes it turning on Y-axis
self._driver.shiftX(output[1])
#angle-speed Z
self._driver.spin(output[2])
logging.debug("PID-angles-speed output: {0}".format(output))
def _readPIDAnglesInput(self):
angles = self._sensor.readDeviceAngles()
logging.debug("PID-angles input: {0}".format(angles))
return angles
def _setPIDAnglesOutput(self, output):
if self._flightMode == FlightController.FLIGHT_MODE_ANGLE:
self._pid.setTarget(output[0], 0) #angle-speed X
self._pid.setTarget(output[1], 1) #angle-speed Y
logging.debug("PID-angles output: {0}".format(output))
def _readPIDAccelInput(self):
inputData = self._sensor.readAccels()
logging.debug("PID-accel input: {0}".format(inputData))
return inputData
def _setPIDAccelOutput(self, output):
#TODO: Implement accel stabilization
#accel Z
#self._driver.addThrottle(output[2])
#logging.debug("PID-accel output: {0}".format(output))
pass
def _readPIDInput(self):
logging.debug("Targets: {0}".format(self._pid.getTargets()))
self._sensor.refreshState()
anglesSpeedInput = self._readPIDAnglesSpeedInput()
anglesInput = self._readPIDAnglesInput()[0:2]
accelInput = self._readPIDAccelInput()
pidInput = anglesSpeedInput + anglesInput + accelInput
return pidInput
def _setPIDOutput(self, output):
anglesSpeedOutput = output[0:3]
anglesOutput = output[3:5]
accelOutput = output[5:8]
self._setPIDAnglesSpeedOutput(anglesSpeedOutput)
self._setPIDAnglesOutput(anglesOutput)
self._setPIDAccelOutput(accelOutput)
self._driver.commitIncrements()
def addThrottle(self, increment):
throttle0 = self._driver.getBaseThrottle()
self._driver.addThrottle(increment)
throttle1 = self._driver.getBaseThrottle()
if throttle1 >= self._pidThrottleThreshold \
and throttle0 < self._pidThrottleThreshold:
#TODO: Stop integrals only
self._startPid()
elif throttle1 < self._pidThrottleThreshold \
and throttle0 >= self._pidThrottleThreshold:
#TODO: Reset and restore integrals here
self._stopPid()
self._driver.setThrottle(throttle1)
if not self._pid.isRunning():
self._driver.commitIncrements()
def setInputs(self, inputs):
'''
Sets the controller inputs. They will be translated into the proper
targets depending of the current flight mode.
Input range is [-100..100]
The order within the inputs-array is as follows:
inputs[0]: roll
inputs[1]: pitch
inputs[2]: rudder
TODO: inputs[3]: throttle (Not implemented)
'''
targets = [
inputs[0] * self._maxAngleX / 100.0,
inputs[1] * self._maxAngleY / 100.0,
inputs[2] * self._maxAngleSpeedZ / 100.0
]
#inputs[3] * Dispatcher.MAX_ACCEL_Z / 100.0]
self._pid.setTarget(targets[0], 3) #angle X
self._pid.setTarget(targets[1], 4) #angle Y
self._pid.setTarget(targets[2], 2) #angle speed Z
#self._pid.setTarget(targets[3], 7) #accel Z
def setTargets(self, targets):
'''
Set the target angles and angle-speeds directly.
@deprecated: Beacuse safety reasons, don't use this method. Please use setInputs instead.
'''
if self._flightMode == FlightController.FLIGHT_MODE_ANGLE:
self._pid.setTarget(targets[0], 3) #angle X
self._pid.setTarget(targets[1], 4) #angle Y
elif self._flightMode == FlightController.FLIGHT_MODE_ANGLE_SPEED:
self._pid.setTarget(targets[0], 0) #angle-speed X
self._pid.setTarget(targets[1], 1) #angle-speed Y
else: #TODO: FlightController.FLIGHT_MODE_ACCEL
raise Exception("Flight mode accel not implemented!")
self._pid.setTarget(targets[2], 2) #angle speed Z
self._pid.setTarget(targets[3], 7) #accel Z
def setFlightMode(self, flightMode):
self._flightMode = flightMode
self._pid.setTargets([0.0] * len(self._pidKP))
# def enableIntegrals(self):
#
# self._pid.enableIntegrals()
#
#
# def disableIntegrals(self):
#
# self._pid.disableIntegrals()
#
def start(self):
if not self._isRunning:
self._isRunning = True
self._sensor.start()
self._driver.start()
self._driver.idle()
def _startPid(self):
if self._isRunning:
print("Starting PID...")
logging.info("Starting PID...")
self._sensor.resetGyroReadTime()
time.sleep(FlightController.PID_PERIOD)
self._sensor.refreshState()
self._pid.start()
def _stopPid(self):
self._pid.stop()
print("PID finished.")
logging.info("PID finished.")
def stop(self):
if self._isRunning:
self._isRunning = False
self._stopPid()
self.idle()
self._driver.stop()
self._sensor.stop()
def standBy(self):
self._stopPid()
self._driver.standBy()
def idle(self):
self._stopPid()
self._driver.idle()
def alterPidAnglesSpeedConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesSpeedKP):
self._pidKP[
axisIndex] = valueP / FlightController.ANGLE_SPEED_FACTOR
self._pidKI[
axisIndex] = valueI / FlightController.ANGLE_SPEED_FACTOR
self._pidKD[
axisIndex] = valueD / FlightController.ANGLE_SPEED_FACTOR
def alterPidAnglesConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesKP):
axisIndex += 3
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
print("constantes: {0}".format([valueP, valueI, valueD]))
def alterPidAccelConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAccelKP):
axisIndex += 5
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
def isRunning(self):
return self._isRunning
def readState(self):
state = State()
state._throttles = self._driver.getThrottles()
state._angles = self._sensor.readDeviceAngles()
state._angles[2] = self._sensor.readAngleSpeeds()[2]
state._accels = self._sensor.readAccels()
state._currentPeriod = self._pid.getCurrentPeriod()
return state
class VisualFlightController(object):
PID_ANGLES_SPEED_PERIOD = 0.05 #seconds
PID_ANGLES_SPEED_KP = 0.0
PID_ANGLES_SPEED_KI = 0.02
PID_ANGLES_SPEED_KD = 0.08
_instance = None
@staticmethod
def getInstance():
if VisualFlightController._instance == None:
VisualFlightController._instance = VisualFlightController()
return VisualFlightController._instance
def __init__(self):
self._driver = Driver()
self._sensor = VisualTracker()
kpMatrix = [VisualFlightController.PID_ANGLES_SPEED_KP] * 4
kiMatrix = [VisualFlightController.PID_ANGLES_SPEED_KI] * 4
kdMatrix = [VisualFlightController.PID_ANGLES_SPEED_KD] * 4
tolerances = [20.0, 20.0, 20.0, 10.0]
initialTargets = [0.0] * 4
self._pidAnglesSpeed = PID(VisualFlightController.PID_ANGLES_SPEED_PERIOD, kpMatrix, kiMatrix, kdMatrix, \
self._readSensor, self._setResult, tolerances, len(kpMatrix))
self._pidAnglesSpeed.setTargets(initialTargets)
self._isRunning = False
def _readSensor(self):
inputData = self._sensor.track()
inputData[2] = 0.0 #Z-input is disabled
return inputData
def _setX(self, increment):
if increment != 0.0:
self._driver.shiftX(increment)
def _setY(self, increment):
if increment != 0.0:
self._driver.shiftY(increment)
def _setZ(self, increment):
if increment != 0.0:
self._driver.addThrottle(increment)
def _setAZ(self, increment):
if increment != 0.0:
self._driver.spin(increment)
def _setResult(self, results):
self._setX(results[0])
self._setY(results[1])
#self._setZ(results[2])
self._setAZ(results[3])
def addThrottle(self, increment):
self._driver.addThrottle(increment)
'''
def shift(self, angle, radius):
acc = VisualFlightController.MAX_ACCELERATION * radius / 100.0
xacc = sin(angle) * acc
yacc = cos(angle) * acc
targetX = VisualFlightController._g2SensorUnit(xacc)
targetY = VisualFlightController._g2SensorUnit(yacc)
self._pidX.setTarget(targetX, 0)
self._pidY.setTarget(targetY, 1)
#logging.debug("Target: {0}".format(self._accTarget))
'''
def start(self):
self._isRunning = True
#self._sensor.start()
self._driver.start()
self._driver.standBy()
self._pidAnglesSpeed.start()
def stop(self):
self._isRunning = False
self._pidAnglesSpeed.stop()
self.standBy()
self._driver.stop()
self._sensor.stop()
def standBy(self):
self._driver.standBy()
def idle(self):
self._driver.idle()
class DriverTestCase(unittest.TestCase):
def setUp(self):
self._driver = Driver(Configuration.VALUE_MOTOR_CLASS_EMULATION)
def test_start(self):
self._driver.start()
throttles = self._driver.getThrottles()
self.assertListEqual(throttles, [0.0] * 4,
"Driver was not properly started")
def test_standBy(self):
self._driver.standBy()
throttles = self._driver.getThrottles()
self.assertListEqual(throttles, [0.0] * 4,
"Driver is not in stand-by mode")
def test_idle(self):
self._driver.idle()
throttles = self._driver.getThrottles()
self.assertListEqual(throttles, [0.0] * 4,
"Driver is not in idle mode")
def test_stop(self):
self._driver.stop()
throttles = self._driver.getThrottles()
self.assertListEqual(throttles, [0.0] * 4,
"Driver was not properly stopped")
def test_addThrottle(self):
self._driver.start()
self._driver.addThrottle(10.0)
self._driver.commitIncrements()
throttles = self._driver.getThrottles()
self.assertListEqual(throttles, [10.0] * 4,
"Driver's motors were not properly incremented")
def test_shiftX(self):
self._driver.start()
self._driver.addThrottle(50.0)
self._driver.shiftX(10.0)
self._driver.commitIncrements()
throttles = self._driver.getThrottles()
#Testing in +-configuration. X-configuration, will issue different result.
self.assertListEqual(throttles, [40.0, 50.0, 60.0, 50.0],
"Driver's motors were not properly shifted")
def test_shiftY(self):
self._driver.start()
self._driver.addThrottle(50.0)
self._driver.shiftY(10.0)
self._driver.commitIncrements()
throttles = self._driver.getThrottles()
#Testing in +-configuration. X-configuration, will issue different result.
self.assertListEqual(throttles, [50.0, 60.0, 50.0, 40.0],
"Driver's motors were not properly shifted")
def test_spin(self):
self._driver.start()
self._driver.addThrottle(50.0)
self._driver.spin(10.0)
self._driver.commitIncrements()
throttles = self._driver.getThrottles()
#Testing in +-configuration. X-configuration, will issue different result.
self.assertListEqual(throttles, [60.0, 40.0, 60.0, 40.0],
"Driver's motors were not properly spinned")
class FlightController(object):
PID_PERIOD = 0.006 #166.666Hz #0.02 # seconds (50Hz)
ANGLE_SPEED_FACTOR = 1000.0 # Angle-speed's PID-constants are divided by this factor
FLIGHT_MODE_ANGLE_SPEED = 0
FLIGHT_MODE_ANGLE = 1
FLIGHT_MODE_ACCEL = 2
_instance = None
@staticmethod
def getInstance():
if FlightController._instance == None:
FlightController._instance = FlightController()
return FlightController._instance
def __init__(self):
self._config = Configuration.getInstance().getConfig()
self._driver = Driver()
self._createSensor(self._config[Configuration.KEY_IMU_CLASS])
#PID constants must have the same length
self._pidAnglesSpeedKP = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KP]]
self._pidAnglesSpeedKI = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KI]]
self._pidAnglesSpeedKD = [k / FlightController.ANGLE_SPEED_FACTOR for k \
in self._config[Configuration.PID_ANGLES_SPEED_KD]]
#PID constants must have the same length
self._pidAnglesKP = self._config[Configuration.PID_ANGLES_KP]
self._pidAnglesKI = self._config[Configuration.PID_ANGLES_KI]
self._pidAnglesKD = self._config[Configuration.PID_ANGLES_KD]
self._pidAccelKP = self._config[Configuration.PID_ACCEL_KP]
self._pidAccelKI = self._config[Configuration.PID_ACCEL_KI]
self._pidAccelKD = self._config[Configuration.PID_ACCEL_KD]
#PID
self._pidKP = self._pidAnglesSpeedKP + self._pidAnglesKP + self._pidAccelKP
self._pidKI = self._pidAnglesSpeedKI + self._pidAnglesKI + self._pidAccelKI
self._pidKD = self._pidAnglesSpeedKD + self._pidAnglesKD + self._pidAccelKD
self._pid = PID(FlightController.PID_PERIOD, \
self._pidKP, self._pidKI, self._pidKD, \
self._readPIDInput, self._setPIDOutput, \
"stabilization-pid")
self._pid.setTargets([0.0]*len(self._pidKP))
self._isRunning = False
self._pidThrottleThreshold = 0
self._maxAngleX = self._config[Configuration.KEY_MAX_ANGLE_X]
self._maxAngleY = self._config[Configuration.KEY_MAX_ANGLE_Y]
#TODO: Rate mode
#self._maxAngleSpeedX = self._config[Configuration.KEY_MAX_ANGLE_SPEED_X] #used for rate mode when implemented
#self._maxAngleSpeedY = self._config[Configuration.KEY_MAX_ANGLE_SPEED_Y] #used for rate mode when implemented
self._maxAngleSpeedZ = self._config[Configuration.KEY_MAX_ANGLE_SPEED_Z]
#TODO: Read initial flight mode from configuration
self._flightMode = FlightController.FLIGHT_MODE_ANGLE
def setPidThrottleThreshold(self, throttle):
self._pidThrottleThreshold = throttle
def _createSensor(self, imuClass):
if imuClass == Configuration.VALUE_IMU_CLASS_3000:
self._sensor = Sensor()
# elif imuClass == Configuration.VALUE_IMU_CLASS_REMOTE:
#
# self._remote = rpyc.classic.connect(self._config[Configuration.KEY_REMOTE_ADDRESS])
# imuDummy = self._remote.modules["sensors.IMU_dummy"]
# self._sensor = imuDummy.IMUDummy()
elif imuClass == Configuration.VALUE_IMU_CLASS_3000EMU:
self._sensor = Imu3000Emulated()
elif imuClass == Configuration.VALUE_IMU_CLASS_6050:
self._sensor = Imu6050Dmp()
elif imuClass == Configuration.VALUE_IMU_CLASS_EMULATION:
self._sensor = EmulatedSensor(EmulatedDrone.REALISTIC_FLIGHT)
else: #Dummy as default
self._sensor = IMUDummy()
def _readPIDAnglesSpeedInput(self):
inputData = self._sensor.readAngleSpeeds()
logging.debug("PID-angles-speed input: {0}".format(inputData))
return inputData
def _setPIDAnglesSpeedOutput(self, output):
#angle X
#Moving the drone along Y-direction makes it turning on X-axis
#Caution! Angle around X-axis as positive sense means moving backwards
self._driver.shiftY(-output[0])
#angle Y
#Moving the drone along X-direction makes it turning on Y-axis
self._driver.shiftX(output[1])
#angle-speed Z
self._driver.spin(output[2])
logging.debug("PID-angles-speed output: {0}".format(output))
def _readPIDAnglesInput(self):
angles = self._sensor.readDeviceAngles()
logging.debug("PID-angles input: {0}".format(angles))
return angles
def _setPIDAnglesOutput(self, output):
if self._flightMode == FlightController.FLIGHT_MODE_ANGLE:
self._pid.setTarget(output[0], 0) #angle-speed X
self._pid.setTarget(output[1], 1) #angle-speed Y
logging.debug("PID-angles output: {0}".format(output))
def _readPIDAccelInput(self):
inputData = self._sensor.readAccels()
logging.debug("PID-accel input: {0}".format(inputData))
return inputData
def _setPIDAccelOutput(self, output):
#TODO: Implement accel stabilization
#accel Z
#self._driver.addThrottle(output[2])
#logging.debug("PID-accel output: {0}".format(output))
pass
def _readPIDInput(self):
logging.debug("Targets: {0}".format(self._pid.getTargets()))
self._sensor.refreshState()
anglesSpeedInput = self._readPIDAnglesSpeedInput()
anglesInput = self._readPIDAnglesInput()[0:2]
accelInput = self._readPIDAccelInput()
pidInput = anglesSpeedInput + anglesInput + accelInput
return pidInput
def _setPIDOutput(self, output):
anglesSpeedOutput = output[0:3]
anglesOutput = output[3:5]
accelOutput = output[5:8]
self._setPIDAnglesSpeedOutput(anglesSpeedOutput)
self._setPIDAnglesOutput(anglesOutput)
self._setPIDAccelOutput(accelOutput)
self._driver.commitIncrements()
def addThrottle(self, increment):
throttle0 = self._driver.getBaseThrottle()
self._driver.addThrottle(increment)
throttle1 = self._driver.getBaseThrottle()
if throttle1 >= self._pidThrottleThreshold \
and throttle0 < self._pidThrottleThreshold:
#TODO: Stop integrals only
self._startPid()
elif throttle1 < self._pidThrottleThreshold \
and throttle0 >= self._pidThrottleThreshold:
#TODO: Reset and restore integrals here
self._stopPid()
self._driver.setThrottle(throttle1)
if not self._pid.isRunning():
self._driver.commitIncrements()
def setInputs(self, inputs):
'''
Sets the controller inputs. They will be translated into the proper
targets depending of the current flight mode.
Input range is [-100..100]
The order within the inputs-array is as follows:
inputs[0]: roll
inputs[1]: pitch
inputs[2]: rudder
TODO: inputs[3]: throttle (Not implemented)
'''
targets = [inputs[0] * self._maxAngleX / 100.0,
inputs[1] * self._maxAngleY / 100.0,
inputs[2] * self._maxAngleSpeedZ / 100.0]
#inputs[3] * Dispatcher.MAX_ACCEL_Z / 100.0]
self._pid.setTarget(targets[0], 3) #angle X
self._pid.setTarget(targets[1], 4) #angle Y
self._pid.setTarget(targets[2], 2) #angle speed Z
#self._pid.setTarget(targets[3], 7) #accel Z
def setTargets(self, targets):
'''
Set the target angles and angle-speeds directly.
@deprecated: Beacuse safety reasons, don't use this method. Please use setInputs instead.
'''
if self._flightMode == FlightController.FLIGHT_MODE_ANGLE:
self._pid.setTarget(targets[0], 3) #angle X
self._pid.setTarget(targets[1], 4) #angle Y
elif self._flightMode == FlightController.FLIGHT_MODE_ANGLE_SPEED:
self._pid.setTarget(targets[0], 0) #angle-speed X
self._pid.setTarget(targets[1], 1) #angle-speed Y
else: #TODO: FlightController.FLIGHT_MODE_ACCEL
raise Exception("Flight mode accel not implemented!")
self._pid.setTarget(targets[2], 2) #angle speed Z
self._pid.setTarget(targets[3], 7) #accel Z
def setFlightMode(self, flightMode):
self._flightMode = flightMode
self._pid.setTargets([0.0]*len(self._pidKP))
# def enableIntegrals(self):
#
# self._pid.enableIntegrals()
#
#
# def disableIntegrals(self):
#
# self._pid.disableIntegrals()
#
def start(self):
if not self._isRunning:
self._isRunning = True
self._sensor.start()
self._driver.start()
self._driver.idle()
def _startPid(self):
if self._isRunning:
print("Starting PID...")
logging.info("Starting PID...")
self._sensor.resetGyroReadTime()
time.sleep(FlightController.PID_PERIOD)
self._sensor.refreshState()
self._pid.start()
def _stopPid(self):
self._pid.stop()
print("PID finished.")
logging.info("PID finished.")
def stop(self):
if self._isRunning:
self._isRunning = False
self._stopPid()
self.idle()
self._driver.stop()
self._sensor.stop()
def standBy(self):
self._stopPid()
self._driver.standBy()
def idle(self):
self._stopPid()
self._driver.idle()
def alterPidAnglesSpeedConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesSpeedKP):
self._pidKP[axisIndex] = valueP / FlightController.ANGLE_SPEED_FACTOR
self._pidKI[axisIndex] = valueI / FlightController.ANGLE_SPEED_FACTOR
self._pidKD[axisIndex] = valueD / FlightController.ANGLE_SPEED_FACTOR
def alterPidAnglesConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAnglesKP):
axisIndex += 3
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
print("constantes: {0}".format([valueP, valueI, valueD]))
def alterPidAccelConstants(self, axisIndex, valueP, valueI, valueD):
if axisIndex < len(self._pidAccelKP):
axisIndex += 5
self._pidKP[axisIndex] = valueP
self._pidKI[axisIndex] = valueI
self._pidKD[axisIndex] = valueD
def isRunning(self):
return self._isRunning
def readState(self):
state = State()
state._throttles = self._driver.getThrottles()
state._angles = self._sensor.readDeviceAngles()
state._angles[2] = self._sensor.readAngleSpeeds()[2]
state._accels = self._sensor.readAccels()
state._currentPeriod = self._pid.getCurrentPeriod()
return state