Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tools: fixed tempcal for when we have gyro cal but no accel cal

This commit is contained in:
Andrew Tridgell 2021-01-17 16:45:13 +11:00 committed by Peter Barker
parent e87dbacee7
commit 6254787c26
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Tools/scripts/tempcal_IMU.py
View File

@ -93,34 +93,36 @@ class Coefficients:
def set_enable(self, imu, value): def set_enable(self, imu, value):
self.enable[imu] = value self.enable[imu] = value
def correction(self, coeff, imu, temperature, axis): def correction(self, coeff, imu, temperature, axis, cal_temp):
'''calculate correction from temperature calibration from log data using parameters''' '''calculate correction from temperature calibration from log data using parameters'''
if self.enable[imu] != 1.0: if self.enable[imu] != 1.0:
return 0.0 return 0.0
if self.atcal[imu] <= -80: if cal_temp < -80:
return 0.0 return 0.0
if not axis in coeff: if not axis in coeff:
return 0.0 return 0.0
temperature = constrain(temperature, self.tmin[imu], self.tmax[imu]) temperature = constrain(temperature, self.tmin[imu], self.tmax[imu])
cal_temp = constrain(self.atcal[imu], self.tmin[imu], self.tmax[imu]) cal_temp = constrain(cal_temp, self.tmin[imu], self.tmax[imu])
poly = np.poly1d(coeff[axis]) poly = np.poly1d(coeff[axis])
ret = poly(self.atcal[imu] - TEMP_REF) - poly(temperature - TEMP_REF) ret = poly(cal_temp - TEMP_REF) - poly(temperature - TEMP_REF)
return ret return ret
def correction_accel(self, imu, temperature): def correction_accel(self, imu, temperature):
'''calculate accel correction from temperature calibration from '''calculate accel correction from temperature calibration from
log data using parameters''' log data using parameters'''
return Vector3(self.correction(self.acoef[imu], imu, temperature, 'X'), cal_temp = self.atcal[imu]
self.correction(self.acoef[imu], imu, temperature, 'Y'), return Vector3(self.correction(self.acoef[imu], imu, temperature, 'X', cal_temp),
self.correction(self.acoef[imu], imu, temperature, 'Z')) self.correction(self.acoef[imu], imu, temperature, 'Y', cal_temp),
self.correction(self.acoef[imu], imu, temperature, 'Z', cal_temp))
def correction_gyro(self, imu, temperature): def correction_gyro(self, imu, temperature):
'''calculate gyro correction from temperature calibration from '''calculate gyro correction from temperature calibration from
log data using parameters''' log data using parameters'''
return Vector3(self.correction(self.gcoef[imu], imu, temperature, 'X'), cal_temp = self.gtcal[imu]
self.correction(self.gcoef[imu], imu, temperature, 'Y'), return Vector3(self.correction(self.gcoef[imu], imu, temperature, 'X', cal_temp),
self.correction(self.gcoef[imu], imu, temperature, 'Z')) self.correction(self.gcoef[imu], imu, temperature, 'Y', cal_temp),
self.correction(self.gcoef[imu], imu, temperature, 'Z', cal_temp))
def param_string(self, imu): def param_string(self, imu):
params = '' params = ''