class FMU(object):
def __init__(self, logfile=None):
'''
Creates a new Quadrotor object with optional logfile.
'''
# Store logfile handle
self.logfile = logfile
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd)
self.roll_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd)
# Special handling for yaw from IMU
self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd)
# Create PD controllers for pitch, roll based on GPS groundspeed (position-hold; hover-in-place)
self.latitude_PID = Hover_PID_Controller(GPS_PITCH_ROLL_Kp)
self.longitude_PID = Hover_PID_Controller(GPS_PITCH_ROLL_Kp)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp)
def getMotors(self, imuAngles, altitude, gpsCoords, controllerInput, timestep):
'''
Gets motor thrusts based on current telemetry:
imuAngles IMU pitch, roll, yaw angles in radians
altitude altitude in meters
gpsCoords GPS coordinates (latitude, longitude) in degrees
controllInput (pitchDemand, rollDemand, yawDemand, climbDemand, switch)
timestep timestep in seconds
'''
# Convert flight-stick controllerInput
pitchDemand = controllerInput[0] * PITCH_DEMAND_FACTOR
rollDemand = controllerInput[1] * ROLL_DEMAND_FACTOR
yawDemand = controllerInput[2] * YAW_DEMAND_FACTOR
climbDemand = controllerInput[3] * CLIMB_DEMAND_FACTOR
# Combine pitch and roll demand into one value for PID control
pitchrollDemand = math.sqrt(pitchDemand**2 + rollDemand**2)
gpsLatCorrection = self.latitude_PID.getCorrection(gpsCoords[0], pitchrollDemand, timestep=timestep)
gpsLongCorrection = self.longitude_PID.getCorrection(gpsCoords[1], pitchrollDemand, timestep=timestep)
# Compute GPS-based pitch, roll correction if we want position-hold
switch = controllerInput[4]
gpsPitchCorrection, gpsRollCorrection = 0,0
if switch == 2:
gpsPitchCorrection, gpsRollCorrection = rotate((gpsLatCorrection, gpsLongCorrection), -imuAngles[2])
gpsPitchCorrection = -gpsPitchCorrection
# Compute altitude hold if we want it
altitudeHold = 0
if switch > 0:
altitudeHold = self.altitude_PID.getCorrection(altitude, timestep=timestep)
# PID control for pitch, roll based on angles from Inertial Measurement Unit (IMU)
imuPitchCorrection = self.pitch_Stability_PID.getCorrection(imuAngles[0], timestep)
imuRollCorrection = self.roll_Stability_PID.getCorrection(-imuAngles[1], timestep)
# Special PID for yaw
yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], yawDemand, timestep)
# Overall pitch, roll correction is sum of stability and position-hold
pitchCorrection = imuPitchCorrection + gpsPitchCorrection
rollCorrection = imuRollCorrection + gpsRollCorrection
# Overall thrust is baseline plus climb demand plus correction from PD controller
thrust = THRUST_BASELINE + climbDemand + altitudeHold
# Change the thrust values depending upon the pitch, roll, yaw and climb values
# received from the joystick and the # quadrotor model corrections. A positive
# pitch value means, thrust increases for two back propellers and a negative
# is opposite; similarly for roll and yaw. A positive climb value means thrust
# increases for all 4 propellers.
psign = [+1, -1, -1, +1]
rsign = [-1, -1, +1, +1]
ysign = [+1, -1, +1, -1]
thrusts = [0]*4
for i in range(4):
thrusts[i] = (thrust + rsign[i]*rollDemand + psign[i]*pitchDemand + ysign[i]*yawDemand) * \
(1 + rsign[i]*rollCorrection + psign[i]*pitchCorrection + ysign[i]*yawCorrection)
return thrusts
class FMU(object):
def __init__(self, logfile=None):
'''
Creates a new Quadrotor object with optional logfile.
'''
# Store logfile handle
self.logfile = logfile
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
self.roll_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
# Special handling for yaw from IMU
self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp, ALTITUDE_Kd)
# For altitude hold
self.switch_prev = 0
self.target_altitude = 0
def getMotors(self, imuAngles, controllerInput, timestep, extraData):
'''
Gets motor thrusts based on current telemetry:
imuAngles IMU pitch, roll, yaw angles in radians (positive = nose up, right down, nose right)
controllerInput (pitchDemand, rollDemand, yawDemand, throttleDemand, switch)
timestep timestep in seconds
extraData extra sensor data for mission
'''
# Convert flight-stick controller input to demands
pitchDemand = controllerInput[0]
rollDemand = controllerInput[1]
yawDemand = -controllerInput[2]
throttleDemand = controllerInput[3]
# Grab value of three-position switch
switch = controllerInput[4]
# Grab altitude from sonar
altitude = extraData[0]
# Lock onto altitude when switch goes on
if switch == 1:
if self.switch_prev == 0:
self.target_altitude = altitude
else:
self.target_altitude = 0
self.switch_prev = switch
# Get PID altitude correction if indicated
altitudeCorrection = self.altitude_PID.getCorrection(altitude, self.target_altitude, timestep) \
if self.target_altitude > 0 \
else 0
# PID control for pitch, roll based on angles from Inertial Measurement Unit (IMU)
imuPitchCorrection = self.pitch_Stability_PID.getCorrection(imuAngles[0], timestep)
imuRollCorrection = self.roll_Stability_PID.getCorrection(-imuAngles[1], timestep)
# Pitch, roll, yaw correction
pitchCorrection = imuPitchCorrection
rollCorrection = imuRollCorrection
yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], yawDemand, timestep)
# Climb demand defaults to throttle demand
climbDemand = throttleDemand
# Adjust climb demand for altitude correction
if altitudeCorrection != 0:
# In deadband, maintain altitude
if throttleDemand > 0.4 and throttleDemand < 0.6:
climbDemand = 0.5 + altitudeCorrection
# Baseline thrust is a nonlinear function of climb demand
thrust = 4*math.sqrt(math.sqrt(climbDemand)) + 2
# Overall thrust is baseline plus throttle demand plus correction from PD controller
# received from the joystick and the # quadrotor model corrections. A positive
# pitch value means, thrust increases for two back propellers and a negative
# is opposite; similarly for roll and yaw. A positive throttle value means thrust
# increases for all 4 propellers.
psign = [+1, -1, -1, +1]
rsign = [-1, -1, +1, +1]
ysign = [+1, -1, +1, -1]
thrusts = [0]*4
for i in range(4):
thrusts[i] = (thrust + rsign[i]*rollDemand*ROLL_DEMAND_FACTOR + \
psign[i]*PITCH_DEMAND_FACTOR*pitchDemand + \
ysign[i]*yawDemand*YAW_DEMAND_FACTOR) * \
(1 + rsign[i]*rollCorrection + psign[i]*pitchCorrection + ysign[i]*yawCorrection)
return thrusts
class FMU(object):
def __init__(self):
'''
Creates a new Quadrotor object.
'''
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
self.roll_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
# Special handling for yaw from IMU
#self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
self.yaw_IMU_PID = Hover_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp, ALTITUDE_Kd)
# Altitude hold z-pisition
self.target_altitude = 0.5
self.target_yaw = 0.0
def getMotors(self, imuAngles, altitude, controllerInput, timestep):
'''
Gets motor thrusts based on current telemetry:
imuAngles IMU pitch, roll, yaw angles in radians
altitude altitude in meters
controllInput (pitchDemand, rollDemand, yawDemand, climbDemand, switch)
timestep timestep in seconds
'''
# Convert flight-stick controllerInput
pitchDemand = controllerInput[0]
rollDemand = controllerInput[1]
#yawDemand = controllerInput[2]
yawDemand = 0.0
#climbDemand = controllerInput[3]
climbDemand = 0.0
# Compute altitude hold if we want it
altitudeCorrection = self.altitude_PID.getCorrection(altitude, self.target_altitude, timestep)
# PID control for pitch, roll based on angles from Inertial Measurement Unit (IMU)
imuPitchCorrection = self.pitch_Stability_PID.getCorrection(imuAngles[0], timestep)
imuRollCorrection = self.roll_Stability_PID.getCorrection(-imuAngles[1], timestep)
# Special PID for yaw
#yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], yawDemand, timestep)
yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], self.target_yaw, timestep)
# Overall pitch, roll correction is sum of stability and position-hold
pitchCorrection = imuPitchCorrection
rollCorrection = imuRollCorrection
if altitudeCorrection != 0:
climbDemand = 0.5 + altitudeCorrection
# Overall thrust is baseline plus climb demand plus correction from PD controller
thrust = 4*math.sqrt(math.sqrt(climbDemand)) + 2
#print('thrust: ' + str(climbDemand) + ' ' + str(altitudeHold) + ' ' + str(altitude))
# Change the thrust values depending upon the pitch, roll, yaw and climb values
# received from the joystick and the # quadrotor model corrections. A positive
# pitch value means, thrust increases for two back propellers and a negative
# is opposite; similarly for roll and yaw. A positive climb value means thrust
# increases for all 4 propellers.
psign = [+1, -1, -1, +1]
rsign = [-1, -1, +1, +1]
ysign = [+1, -1, +1, -1]
thrusts = [0]*4
for i in range(4):
thrusts[i] = (thrust + rsign[i]*rollDemand*ROLL_DEMAND_FACTOR \
+ psign[i]*pitchDemand*PITCH_DEMAND_FACTOR \
+ ysign[i]*yawDemand*YAW_DEMAND_FACTOR)*(1 + rsign[i]*rollCorrection + psign[i]*pitchCorrection + ysign[i]*yawCorrection)
return thrusts
class FMU(object):
def __init__(self):
'''
Creates a new Quadrotor object.
'''
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(
IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
self.roll_Stability_PID = Stability_PID_Controller(
IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
# Special handling for yaw from IMU
#self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
self.yaw_IMU_PID = Hover_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd,
IMU_YAW_Ki)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp, ALTITUDE_Kd)
# Altitude hold z-pisition
self.target_altitude = 0.5
self.target_yaw = 0.0
def getMotors(self, imuAngles, altitude, controllerInput, timestep):
'''
Gets motor thrusts based on current telemetry:
imuAngles IMU pitch, roll, yaw angles in radians
altitude altitude in meters
controllInput (pitchDemand, rollDemand, yawDemand, climbDemand, switch)
timestep timestep in seconds
'''
# Convert flight-stick controllerInput
pitchDemand = controllerInput[0]
rollDemand = controllerInput[1]
#yawDemand = controllerInput[2]
yawDemand = 0.0
#climbDemand = controllerInput[3]
climbDemand = 0.0
# Compute altitude hold if we want it
altitudeCorrection = self.altitude_PID.getCorrection(
altitude, self.target_altitude, timestep)
# PID control for pitch, roll based on angles from Inertial Measurement Unit (IMU)
imuPitchCorrection = self.pitch_Stability_PID.getCorrection(
imuAngles[0], timestep)
imuRollCorrection = self.roll_Stability_PID.getCorrection(
-imuAngles[1], timestep)
# Special PID for yaw
#yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], yawDemand, timestep)
yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2],
self.target_yaw,
timestep)
# Overall pitch, roll correction is sum of stability and position-hold
pitchCorrection = imuPitchCorrection
rollCorrection = imuRollCorrection
if altitudeCorrection != 0:
climbDemand = 0.5 + altitudeCorrection
# Overall thrust is baseline plus climb demand plus correction from PD controller
thrust = 4 * math.sqrt(math.sqrt(climbDemand)) + 2
#print('thrust: ' + str(climbDemand) + ' ' + str(altitudeHold) + ' ' + str(altitude))
# Change the thrust values depending upon the pitch, roll, yaw and climb values
# received from the joystick and the # quadrotor model corrections. A positive
# pitch value means, thrust increases for two back propellers and a negative
# is opposite; similarly for roll and yaw. A positive climb value means thrust
# increases for all 4 propellers.
psign = [+1, -1, -1, +1]
rsign = [-1, -1, +1, +1]
ysign = [+1, -1, +1, -1]
thrusts = [0] * 4
for i in range(4):
thrusts[i] = (thrust + rsign[i]*rollDemand*ROLL_DEMAND_FACTOR \
+ psign[i]*pitchDemand*PITCH_DEMAND_FACTOR \
+ ysign[i]*yawDemand*YAW_DEMAND_FACTOR)*(1 + rsign[i]*rollCorrection + psign[i]*pitchCorrection + ysign[i]*yawCorrection)
return thrusts