diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
commit | 8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch) | |
tree | 4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /mavlink/share/pyshared/pymavlink/mavextra.py | |
download | px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.gz px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.bz2 px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.zip |
Fresh import of the PX4 firmware sources.
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/mavextra.py')
-rw-r--r-- | mavlink/share/pyshared/pymavlink/mavextra.py | 154 |
1 files changed, 154 insertions, 0 deletions
diff --git a/mavlink/share/pyshared/pymavlink/mavextra.py b/mavlink/share/pyshared/pymavlink/mavextra.py new file mode 100644 index 000000000..5395f6036 --- /dev/null +++ b/mavlink/share/pyshared/pymavlink/mavextra.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python +''' +useful extra functions for use by mavlink clients + +Copyright Andrew Tridgell 2011 +Released under GNU GPL version 3 or later +''' + +from math import * + + +def kmh(mps): + '''convert m/s to Km/h''' + return mps*3.6 + +def altitude(press_abs, ground_press=955.0, ground_temp=30): + '''calculate barometric altitude''' + return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001 + + +def mag_heading(RAW_IMU, ATTITUDE, declination=0, SENSOR_OFFSETS=None, ofs=None): + '''calculate heading from raw magnetometer''' + mag_x = RAW_IMU.xmag + mag_y = RAW_IMU.ymag + mag_z = RAW_IMU.zmag + if SENSOR_OFFSETS is not None and ofs is not None: + mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x + mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y + mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z + + headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch) + headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll) + heading = degrees(atan2(-headY,headX)) + declination + if heading < 0: + heading += 360 + return heading + +def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None): + '''calculate magnetic field strength from raw magnetometer''' + mag_x = RAW_IMU.xmag + mag_y = RAW_IMU.ymag + mag_z = RAW_IMU.zmag + if SENSOR_OFFSETS is not None and ofs is not None: + mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x + mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y + mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z + return sqrt(mag_x**2 + mag_y**2 + mag_z**2) + +def angle_diff(angle1, angle2): + '''show the difference between two angles in degrees''' + ret = angle1 - angle2 + if ret > 180: + ret -= 360; + if ret < -180: + ret += 360 + return ret + + +lowpass_data = {} + +def lowpass(var, key, factor): + '''a simple lowpass filter''' + global lowpass_data + if not key in lowpass_data: + lowpass_data[key] = var + else: + lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var + return lowpass_data[key] + +last_delta = {} + +def delta(var, key): + '''calculate slope''' + global last_delta + dv = 0 + if key in last_delta: + dv = var - last_delta[key] + last_delta[key] = var + return dv + +def delta_angle(var, key): + '''calculate slope of an angle''' + global last_delta + dv = 0 + if key in last_delta: + dv = var - last_delta[key] + last_delta[key] = var + if dv > 180: + dv -= 360 + if dv < -180: + dv += 360 + return dv + +def roll_estimate(RAW_IMU,smooth=0.7): + '''estimate roll from accelerometer''' + rx = lowpass(RAW_IMU.xacc,'rx',smooth) + ry = lowpass(RAW_IMU.yacc,'ry',smooth) + rz = lowpass(RAW_IMU.zacc,'rz',smooth) + return degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))) + +def pitch_estimate(RAW_IMU, smooth=0.7): + '''estimate pitch from accelerometer''' + rx = lowpass(RAW_IMU.xacc,'rx',smooth) + ry = lowpass(RAW_IMU.yacc,'ry',smooth) + rz = lowpass(RAW_IMU.zacc,'rz',smooth) + return degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))) + +def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, smooth=0.7): + '''estimate pitch from accelerometer''' + rx = RAW_IMU.xacc + ry = RAW_IMU.yacc + rz = RAW_IMU.zacc+45 + if SENSOR_OFFSETS is not None and ofs is not None: + rx += ofs[0] - SENSOR_OFFSETS.accel_cal_x + ry += ofs[1] - SENSOR_OFFSETS.accel_cal_y + rz += ofs[2] - SENSOR_OFFSETS.accel_cal_z + return lowpass(sqrt(rx**2+ry**2+rz**2)*0.01,'_gravity',smooth) + + + +def pitch_sim(SIMSTATE, GPS_RAW): + '''estimate pitch from SIMSTATE accels''' + xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9) + zacc = SIMSTATE.zacc + zacc += SIMSTATE.ygyro * GPS_RAW.v; + if xacc/zacc >= 1: + return 0 + if xacc/zacc <= -1: + return -0 + return degrees(-asin(xacc/zacc)) + +def distance_two(GPS_RAW1, GPS_RAW2): + '''distance between two points''' + lat1 = radians(GPS_RAW1.lat) + lat2 = radians(GPS_RAW2.lat) + lon1 = radians(GPS_RAW1.lon) + lon2 = radians(GPS_RAW2.lon) + dLat = lat2 - lat1 + dLon = lon2 - lon1 + + a = sin(0.5*dLat) * sin(0.5*dLat) + sin(0.5*dLon) * sin(0.5*dLon) * cos(lat1) * cos(lat2) + c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) + return 6371 * 1000 * c + + +first_fix = None + +def distance_home(GPS_RAW): + '''distance from first fix point''' + global first_fix + if first_fix == None or first_fix.fix_type < 2: + first_fix = GPS_RAW + return 0 + return distance_two(GPS_RAW, first_fix) |