예제 #1
0
파일: fmu.py 프로젝트: hughhugh/dqn-vrep
    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
예제 #2
0
파일: fmu.py 프로젝트: benzeng/PyQuadSim
    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)
예제 #3
0
    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
예제 #4
0
    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
예제 #5
0
파일: fmu.py 프로젝트: benzeng/PyQuadSim
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
예제 #6
0
파일: fmu.py 프로젝트: hughhugh/dqn-vrep
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
예제 #7
0
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
예제 #8
0
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