# magCalScaleFactor = np.array([0.9871891891891892, 0.997296928327645, 1.063218920557914])
#BAY
magCalBias = np.array([-25.1875, -9.90625, 16.625])
magCalScaleFactor = np.array(
[0.9729456159822419, 0.972405990016639, 1.0473405017921147])
myIMU.magCalBias = magCalBias
myIMU.magCalScaleFactor = magCalScaleFactor
C_orient = np.zeros((3, 3))
C_orient[0][2] = -1.0
C_orient[1][1] = -1.0
C_orient[2][0] = -1.0
myIMU.setOrientation(C_orient)
C_Device2Sensor = C_orient.transpose()
myAHRS = AHRS(myIMU)
# (t,r,a,m) = myIMU.sample()
# Colors
BLACK = 0
WHITE = 255
# Parameters to Change
BORDER = 3
FONTSIZE = 50
spi = busio.SPI(board.SCK, MOSI=board.MOSI)
scs = digitalio.DigitalInOut(board.D22) # inverted chip select
# display = adafruit_sharpmemorydisplay.SharpMemoryDisplay(spi, scs, 96, 96)
acc_magFilt = signal.filtfilt(b, a, acc_magFilt)
stationary = acc_magFilt < 0.10
plt.plot(time, accX, 'r', linewidth=.5)
plt.plot(time, accY, 'g', linewidth=.5)
plt.plot(time, accZ, 'b', linewidth=.5)
plt.plot(time, acc_magFilt, ':k', linewidth=.5)
# plt.plot(time,stationary,'k', linewidth=2)
plt.show()
# -------------------------------------------------------------------------
# Compute orientation
quat = []
AHRSalgorithm = AHRS()
# Initial convergence
initPeriod = 2
gyroHold = np.array([0, 0, 0])
index = int(initPeriod / samplePeriod) + 1
index = 8
accHold = [np.mean(accX[:index]), np.mean(accY[:index]), np.mean(accZ[:index])]
for x in range(2000):
AHRSalgorithm.UpdateIMU(gyroHold, accHold)
for i in range(len(accX)):
if stationary[i]:
AHRSalgorithm.Kp = 0.5
else:
AHRSalgorithm.Kp = 0
def get_magneticfieldCaption():
global AHRS
response = jsonify(AHRS.getDisplayableMagneticFieldCaption())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
# magCalBias = np.array([-19, 2.5, 16.625]) # magCalScaleFactor = np.array([1.0, 1.0, 1.0]) # magCalBias = np.array([0.0, 0.0, 0.0]) magCalScaleFactor = np.array([1.0, 1.0, 1.0]) myIMU.magCalBias = magCalBias myIMU.magCalScaleFactor = magCalScaleFactor C_orient = np.zeros((3, 3)) C_orient[0][2] = -1.0 C_orient[1][1] = -1.0 C_orient[2][0] = -1.0 myIMU.setOrientation(C_orient) C_Device2Sensor = C_orient.transpose() myAHRS = AHRS(myIMU) calFirstFrame = 0 horzMag = 17.679 vertMag = 51.9472 fieldMag = 54.789 xMax = -101 yMax = -101 zMax = -101 xMin = 100 yMin = 100 zMin = 100 xScale = 1 yScale = 1 zScale = 1
def get_accelerationCaption():
global AHRS
response = jsonify(AHRS.getDisplayableAccelerationCaption())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_angularspeedCaption():
global AHRS
response = jsonify(AHRS.getDisplayableAngularSpeedCaption())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_imu():
global AHRS
response = jsonify(AHRS.getIMUData())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_attitude():
global AHRS
response = jsonify(AHRS.getAttitudeData())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def put_calibrate_accelerometer():
global AHRS
AHRS.calibrateAccelerometer()
response = jsonify({'status': 'OK'})
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_accelerometer_calibration_statusget_calibration_status():
global AHRS
response = jsonify(AHRS.getAccelerometerCalibrationStatus())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_magnetometer_tl():
global AHRS
response = jsonify(AHRS.getDisplayableMagnetometerTL())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
def get_euler():
global AHRS
response = jsonify(AHRS.getDisplayableEuler())
response.headers.add('Access-Control-Allow-Origin', '*')
return response
import random
import threading
import json
import math
from flask import Flask, jsonify, request, render_template
from ServoController import ServoController
from AHRS import AHRS
from Controller import Controller
from PendulumSimulator import PendulumSimulator
# Use for web server
app = Flask(__name__)
# Use for Attitude computation
AHRS = AHRS.AHRS(period=0.6)
servos = ServoController.ServoController(11, 13, 14)
pendule = PendulumSimulator.PendulumSimulator(sysAngle=4 * math.pi / 5)
@app.route('/display', methods=['GET'])
def get_displayRoutes():
routes = []
for rule in app.url_map.iter_rules():
if "/display/" in str(rule) and "/caption" not in str(rule):
route = {}
route['route'] = str(rule)
route['title'] = str(rule).split('/')[-1].capitalize()
routes.append(route)
response = jsonify(routes)
response.headers.add('Access-Control-Allow-Origin', '*')