def calibrate():
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly
AA = 0.40 # Complementary filter constant
gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0
CFangleZ = 0.0
pressure = 0.0
altitude = 0.0
IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected.
IMU.initIMU() #Initialise the accelerometer, gyroscope and compass
a = datetime.datetime.now()
cal_length = 300
X_cal = [0.0] * cal_length
Y_cal = [0.0] * cal_length
Z_cal = [0.0] * cal_length
Head_cal = [0.0] * cal_length
#cal_ready = raw_input("Is the plane level and ready for IMU calibration (Y/N)? ")
#type(cal_ready)
cal_ready = 'Y'
if (cal_ready == 'Y'):
print('Entering Calibration Routine')
for i in range(0, cal_length):
ACCx = IMU.readACCx()
ACCy = IMU.readACCy()
ACCz = IMU.readACCz()
GYRx = IMU.readGYRx()
GYRy = IMU.readGYRy()
GYRz = IMU.readGYRz()
MAGx = IMU.readMAGx()
MAGy = IMU.readMAGy()
MAGz = IMU.readMAGz()
#pressure = bmp280driver.readALT()
##Calculate loop Period(LP). How long between Gyro Reads
b = datetime.datetime.now() - a
a = datetime.datetime.now()
LP = b.microseconds / (1000000 * 1.0)
#print "Loop Time | %5.2f|" % ( LP ),
#Convert Gyro raw to degrees per second
rate_gyr_x = GYRx * G_GAIN
rate_gyr_y = GYRy * G_GAIN
rate_gyr_z = GYRz * G_GAIN
#Calculate the angles from the gyro.
gyroXangle += rate_gyr_x * LP
gyroYangle += rate_gyr_y * LP
gyroZangle += rate_gyr_z * LP
#Convert Accelerometer values to degrees
AccXangle = (math.atan2(ACCy, ACCz) + M_PI) * RAD_TO_DEG
AccYangle = (math.atan2(ACCz, ACCx) + M_PI) * RAD_TO_DEG
AccZangle = (math.atan2(ACCx, ACCy) + M_PI) * RAD_TO_DEG
#convert the values to -180 and +180
AccXangle -= 180.0
AccZangle -= 180.0
if AccYangle > 90:
AccYangle -= 270.0
else:
AccYangle += 90.0
#Complementary filter used to combine the accelerometer and gyro values.
CFangleX = AA * (CFangleX + rate_gyr_x * LP) + (1 - AA) * AccXangle
CFangleY = AA * (CFangleY + rate_gyr_y * LP) + (1 - AA) * AccYangle
CFangleZ = AA * (CFangleZ + rate_gyr_z * LP) + (1 - AA) * AccZangle
# gyro filter array
X_cal[i] = CFangleX
Y_cal[i] = CFangleY
Z_cal[i] = CFangleZ
#Calculate heading
heading = 180 * math.atan2(MAGy, MAGx) / M_PI
# magnetometer filter array
Head_cal[i] = heading
X_offset = sum(X_cal) / max(len(X_cal), 1)
Y_offset = sum(Y_cal) / max(len(Y_cal), 1)
Z_offset = sum(Z_cal) / max(len(Z_cal), 1)
Head_offset = sum(Head_cal) / max(len(Head_cal), 1)
heading_command = Head_offset
print("Calibration Complete")
print("X_offset = %5.2f" % (X_offset))
print("Y_offset = %5.2f" % (Y_offset))
print("Z_offset = %5.2f" % (Z_offset))
print("Head_offset = %5.2f" % (Head_offset))
return (X_offset, Y_offset, Z_offset, Head_offset, pressure)
def __init__(self):
# Initialise the PCA9685 using the default address (0x40).
#self.pwm = Adafruit_PCA9685.PCA9685()
# Set frequency to 60hz, good for servos.
#self.pwm.set_pwm_freq(60)
self.throttleServo = servo.Servo(0, throttle_servo_min,
throttle_servo_max, false)
self.aileronServo = servo.Servo(1, aileron_servo_min,
aileron_servo_max, false)
self.elevatorServo = servo.Servo(2, elevator_servo_min,
elevator_servo_max, true)
self.rudderServo = servo.Servo(0, rudder_servo_min, rudder_servo_max,
false)
IMU.detectIMU()
IMU.writeACC(LSM9DS1_CTRL_REG7_XL, 0b11100001)
self.gyroXangle = 0.0
self.gyroYangle = 0.0
self.gyroZangle = 0.0
self.CFangleX = 0.0
self.CFangleY = 0.0
self.CFangleZ = 0.0 # Uneeded in new?
self.CFangleXFiltered = 0.0
self.CFangleYFiltered = 0.0
self.kalmanX = 0.0
self.kalmanY = 0.0
self.oldXMagRawValue = 0
self.oldYMagRawValue = 0
self.oldZMagRawValue = 0
self.oldXAccRawValue = 0
self.oldYAccRawValue = 0
self.oldZAccRawValue = 0
# Setup the tables for the median filter. Fill them all with '1' so we don't get a divide by zero error
self.acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE
self.mag_medianTable1X = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable1Y = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable1Z = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2X = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2Y = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2Z = [1] * MAG_MEDIANTABLESIZE
# Kalman filter variables
self.y_bias = 0.0
self.x_bias = 0.0
self.XP_00 = 0.0
self.XP_01 = 0.0
self.XP_10 = 0.0
self.XP_11 = 0.0
self.YP_00 = 0.0
self.YP_01 = 0.0
self.YP_10 = 0.0
self.YP_11 = 0.0
self.KFangleX = 0.0
self.KFangleY = 0.0
self.pressure = 0.0
self.altitude = 0.0
# Moving average filter settings for Roll, Pitch, Yaw readings
self.combined_filter_length = 1
self.CFangleX_filter_length = self.combined_filter_length
self.CFangleY_filter_length = self.combined_filter_length
self.CFangleZ_filter_length = self.combined_filter_length
self.tiltHeading_filter_length = self.combined_filter_length
self.CFangleX_filtered = 0.0
self.CFangleX_reading = [0.0] * self.CFangleX_filter_length
self.CFangleY_filtered = 0.0
self.CFangleY_reading = [0.0] * self.CFangleY_filter_length
self.CFangleZ_filtered = 0.0
self.CFangleZ_reading = [0.0] * self.CFangleZ_filter_length
self.tiltHeading_filtered = 0.0
self.tiltHeading_reading = [0.0] * self.tiltHeading_filter_length
self.time_a = datetime.datetime.now()
heading_d_gain = 0.0
yaw_d_gain = 0.0
roll_d_gain = 0.0
pitch_d_gain = 0.0
velocity_d_gain = 0.0
rudder_mid_pos = (rudder_servo_max + rudder_servo_min) / 2
aileron_mid_pos = (aileron_servo_max + aileron_servo_min) / 2
elevator_mid_pos = (elevator_servo_max + elevator_servo_min) / 2
rudder_command = 0
aileron_command = 0
elevator_command = 0
throttle_command = 0
IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected.
IMU.initIMU() #Initialise the accelerometer, gyroscope and compass
# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(channel, pulse):
pulse_length = 1000000 # 1,000,000 us per second
pulse_length //= 60 # 60 Hz
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096 # 12 bits of resolution
print('{0}us per bit'.format(pulse_length))
pulse *= 1000
pulse //= pulse_length
pwm.set_pwm(channel, 0, pulse)
