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