mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +08:00
Visualization code has been added.
This commit is contained in:
Hyon Lim (Retina)
@@ -0,0 +1,5 @@
|
||||
Synopsis
|
||||
|
||||
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
|
||||
|
||||
Option -d is for debugging packet. See visualization_code folder.
|
||||
@@ -1,7 +1,19 @@
|
||||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.c
|
||||
*
|
||||
* Parameters for SO3 complementary filter
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_so3_comp_params.h"
|
||||
|
||||
@@ -1,7 +1,19 @@
|
||||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.h
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
+265
@@ -0,0 +1,265 @@
|
||||
/**
|
||||
Visualize a cube which will assumes the orientation described
|
||||
in a quaternion coming from the serial port.
|
||||
|
||||
INSTRUCTIONS:
|
||||
This program has to be run when you have the FreeIMU_serial
|
||||
program running on your Arduino and the Arduino connected to your PC.
|
||||
Remember to set the serialPort variable below to point to the name the
|
||||
Arduino serial port has in your system. You can get the port using the
|
||||
Arduino IDE from Tools->Serial Port: the selected entry is what you have
|
||||
to use as serialPort variable.
|
||||
|
||||
|
||||
Copyright (C) 2011-2012 Fabio Varesano - http://www.varesano.net/
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the version 3 GNU General Public License as
|
||||
published by the Free Software Foundation.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
import processing.serial.*;
|
||||
import processing.opengl.*;
|
||||
|
||||
Serial myPort; // Create object from Serial class
|
||||
|
||||
final String serialPort = "/dev/ttyUSB9"; // replace this with your serial port. On windows you will need something like "COM1".
|
||||
|
||||
float [] q = new float [4];
|
||||
float [] hq = null;
|
||||
float [] Euler = new float [3]; // psi, theta, phi
|
||||
|
||||
int lf = 10; // 10 is '\n' in ASCII
|
||||
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)
|
||||
|
||||
PFont font;
|
||||
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
|
||||
|
||||
final int burst = 32;
|
||||
int count = 0;
|
||||
|
||||
void myDelay(int time) {
|
||||
try {
|
||||
Thread.sleep(time);
|
||||
} catch (InterruptedException e) { }
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
size(VIEW_SIZE_X, VIEW_SIZE_Y, OPENGL);
|
||||
myPort = new Serial(this, serialPort, 115200);
|
||||
|
||||
// The font must be located in the sketch's "data" directory to load successfully
|
||||
font = loadFont("CourierNew36.vlw");
|
||||
|
||||
println("Waiting IMU..");
|
||||
|
||||
myPort.clear();
|
||||
|
||||
while (myPort.available() == 0) {
|
||||
myPort.write("v");
|
||||
myDelay(1000);
|
||||
}
|
||||
println(myPort.readStringUntil('\n'));
|
||||
myPort.write("q" + char(burst));
|
||||
myPort.bufferUntil('\n');
|
||||
}
|
||||
|
||||
|
||||
float decodeFloat(String inString) {
|
||||
byte [] inData = new byte[4];
|
||||
|
||||
if(inString.length() == 8) {
|
||||
inData[0] = (byte) unhex(inString.substring(0, 2));
|
||||
inData[1] = (byte) unhex(inString.substring(2, 4));
|
||||
inData[2] = (byte) unhex(inString.substring(4, 6));
|
||||
inData[3] = (byte) unhex(inString.substring(6, 8));
|
||||
}
|
||||
|
||||
int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
|
||||
return Float.intBitsToFloat(intbits);
|
||||
}
|
||||
|
||||
void serialEvent(Serial p) {
|
||||
if(p.available() >= 18) {
|
||||
String inputString = p.readStringUntil('\n');
|
||||
//print(inputString);
|
||||
if (inputString != null && inputString.length() > 0) {
|
||||
String [] inputStringArr = split(inputString, ",");
|
||||
if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
|
||||
q[0] = decodeFloat(inputStringArr[0]);
|
||||
q[1] = decodeFloat(inputStringArr[1]);
|
||||
q[2] = decodeFloat(inputStringArr[2]);
|
||||
q[3] = decodeFloat(inputStringArr[3]);
|
||||
}
|
||||
}
|
||||
count = count + 1;
|
||||
if(burst == count) { // ask more data when burst completed
|
||||
p.write("q" + char(burst));
|
||||
count = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void buildBoxShape() {
|
||||
//box(60, 10, 40);
|
||||
noStroke();
|
||||
beginShape(QUADS);
|
||||
|
||||
//Z+ (to the drawing area)
|
||||
fill(#00ff00);
|
||||
vertex(-30, -5, 20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
|
||||
//Z-
|
||||
fill(#0000ff);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, 5, -20);
|
||||
vertex(-30, 5, -20);
|
||||
|
||||
//X-
|
||||
fill(#ff0000);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(-30, -5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
vertex(-30, 5, -20);
|
||||
|
||||
//X+
|
||||
fill(#ffff00);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(30, 5, -20);
|
||||
|
||||
//Y-
|
||||
fill(#ff00ff);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(-30, -5, 20);
|
||||
|
||||
//Y+
|
||||
fill(#00ffff);
|
||||
vertex(-30, 5, -20);
|
||||
vertex(30, 5, -20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
|
||||
endShape();
|
||||
}
|
||||
|
||||
|
||||
void drawCube() {
|
||||
pushMatrix();
|
||||
translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
|
||||
scale(5,5,5);
|
||||
|
||||
// a demonstration of the following is at
|
||||
// http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
|
||||
rotateZ(-Euler[2]);
|
||||
rotateX(-Euler[1]);
|
||||
rotateY(-Euler[0]);
|
||||
|
||||
buildBoxShape();
|
||||
|
||||
popMatrix();
|
||||
}
|
||||
|
||||
|
||||
void draw() {
|
||||
background(#000000);
|
||||
fill(#ffffff);
|
||||
|
||||
if(hq != null) { // use home quaternion
|
||||
quaternionToEuler(quatProd(hq, q), Euler);
|
||||
text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
|
||||
}
|
||||
else {
|
||||
quaternionToEuler(q, Euler);
|
||||
text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
|
||||
}
|
||||
|
||||
textFont(font, 20);
|
||||
textAlign(LEFT, TOP);
|
||||
text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
|
||||
text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " + degrees(Euler[2]), 200, 20);
|
||||
|
||||
drawCube();
|
||||
//myPort.write("q" + 1);
|
||||
}
|
||||
|
||||
|
||||
void keyPressed() {
|
||||
if(key == 'h') {
|
||||
println("pressed h");
|
||||
|
||||
// set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
|
||||
hq = quatConjugate(q);
|
||||
|
||||
}
|
||||
else if(key == 'n') {
|
||||
println("pressed n");
|
||||
hq = null;
|
||||
}
|
||||
}
|
||||
|
||||
// See Sebastian O.H. Madwick report
|
||||
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
|
||||
|
||||
void quaternionToEuler(float [] q, float [] euler) {
|
||||
euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
|
||||
euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
|
||||
euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
|
||||
}
|
||||
|
||||
float [] quatProd(float [] a, float [] b) {
|
||||
float [] q = new float[4];
|
||||
|
||||
q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
|
||||
q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
|
||||
q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
|
||||
q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
// returns a quaternion from an axis angle representation
|
||||
float [] quatAxisAngle(float [] axis, float angle) {
|
||||
float [] q = new float[4];
|
||||
|
||||
float halfAngle = angle / 2.0;
|
||||
float sinHalfAngle = sin(halfAngle);
|
||||
q[0] = cos(halfAngle);
|
||||
q[1] = -axis[0] * sinHalfAngle;
|
||||
q[2] = -axis[1] * sinHalfAngle;
|
||||
q[3] = -axis[2] * sinHalfAngle;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
// return the quaternion conjugate of quat
|
||||
float [] quatConjugate(float [] quat) {
|
||||
float [] conj = new float[4];
|
||||
|
||||
conj[0] = quat[0];
|
||||
conj[1] = -quat[1];
|
||||
conj[2] = -quat[2];
|
||||
conj[3] = -quat[3];
|
||||
|
||||
return conj;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
+229
@@ -0,0 +1,229 @@
|
||||
/**
|
||||
Visualize orientation information from a FreeIMU device
|
||||
|
||||
INSTRUCTIONS:
|
||||
This program has to be run when you have the FreeIMU_serial
|
||||
program running on your Arduino and the Arduino connected to your PC.
|
||||
Remember to set the serialPort variable below to point to the name the
|
||||
Arduino serial port has in your system. You can get the port using the
|
||||
Arduino IDE from Tools->Serial Port: the selected entry is what you have
|
||||
to use as serialPort variable.
|
||||
|
||||
Copyright (C) 2011-2012 Fabio Varesano - http://www.varesano.net/
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the version 3 GNU General Public License as
|
||||
published by the Free Software Foundation.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
*/
|
||||
|
||||
import processing.serial.*;
|
||||
|
||||
Serial myPort; // Create object from Serial class
|
||||
|
||||
|
||||
float [] q = new float [4];
|
||||
float [] Euler = new float [3]; // psi, theta, phi
|
||||
|
||||
int lf = 10; // 10 is '\n' in ASCII
|
||||
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)
|
||||
|
||||
PFont font;
|
||||
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
|
||||
|
||||
int burst = 32, count = 0;
|
||||
|
||||
void myDelay(int time) {
|
||||
try {
|
||||
Thread.sleep(time);
|
||||
} catch (InterruptedException e) { }
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
|
||||
myPort = new Serial(this, "/dev/ttyUSB9", 115200);
|
||||
|
||||
// The font must be located in the sketch's "data" directory to load successfully
|
||||
font = loadFont("CourierNew36.vlw");
|
||||
|
||||
println("Waiting IMU..");
|
||||
|
||||
myPort.clear();
|
||||
|
||||
while (myPort.available() == 0) {
|
||||
myPort.write("v");
|
||||
myDelay(1000);
|
||||
}
|
||||
println(myPort.readStringUntil('\n'));
|
||||
myPort.write("q" + char(burst));
|
||||
myPort.bufferUntil('\n');
|
||||
}
|
||||
|
||||
|
||||
float decodeFloat(String inString) {
|
||||
byte [] inData = new byte[4];
|
||||
|
||||
if(inString.length() == 8) {
|
||||
inData[0] = (byte) unhex(inString.substring(0, 2));
|
||||
inData[1] = (byte) unhex(inString.substring(2, 4));
|
||||
inData[2] = (byte) unhex(inString.substring(4, 6));
|
||||
inData[3] = (byte) unhex(inString.substring(6, 8));
|
||||
}
|
||||
|
||||
int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
|
||||
return Float.intBitsToFloat(intbits);
|
||||
}
|
||||
|
||||
|
||||
void serialEvent(Serial p) {
|
||||
if(p.available() >= 18) {
|
||||
String inputString = p.readStringUntil('\n');
|
||||
//print(inputString);
|
||||
if (inputString != null && inputString.length() > 0) {
|
||||
String [] inputStringArr = split(inputString, ",");
|
||||
if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
|
||||
q[0] = decodeFloat(inputStringArr[0]);
|
||||
q[1] = decodeFloat(inputStringArr[1]);
|
||||
q[2] = decodeFloat(inputStringArr[2]);
|
||||
q[3] = decodeFloat(inputStringArr[3]);
|
||||
}
|
||||
}
|
||||
count = count + 1;
|
||||
if(burst == count) { // ask more data when burst completed
|
||||
p.write("q" + char(burst));
|
||||
count = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void buildBoxShape() {
|
||||
//box(60, 10, 40);
|
||||
noStroke();
|
||||
beginShape(QUADS);
|
||||
|
||||
//Z+ (to the drawing area)
|
||||
fill(#00ff00);
|
||||
vertex(-30, -5, 20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
|
||||
//Z-
|
||||
fill(#0000ff);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, 5, -20);
|
||||
vertex(-30, 5, -20);
|
||||
|
||||
//X-
|
||||
fill(#ff0000);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(-30, -5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
vertex(-30, 5, -20);
|
||||
|
||||
//X+
|
||||
fill(#ffff00);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(30, 5, -20);
|
||||
|
||||
//Y-
|
||||
fill(#ff00ff);
|
||||
vertex(-30, -5, -20);
|
||||
vertex(30, -5, -20);
|
||||
vertex(30, -5, 20);
|
||||
vertex(-30, -5, 20);
|
||||
|
||||
//Y+
|
||||
fill(#00ffff);
|
||||
vertex(-30, 5, -20);
|
||||
vertex(30, 5, -20);
|
||||
vertex(30, 5, 20);
|
||||
vertex(-30, 5, 20);
|
||||
|
||||
endShape();
|
||||
}
|
||||
|
||||
|
||||
void drawcompass(float heading, int circlex, int circley, int circlediameter) {
|
||||
noStroke();
|
||||
ellipse(circlex, circley, circlediameter, circlediameter);
|
||||
fill(#ff0000);
|
||||
ellipse(circlex, circley, circlediameter/20, circlediameter/20);
|
||||
stroke(#ff0000);
|
||||
strokeWeight(4);
|
||||
line(circlex, circley, circlex - circlediameter/2 * sin(-heading), circley - circlediameter/2 * cos(-heading));
|
||||
noStroke();
|
||||
fill(#ffffff);
|
||||
textAlign(CENTER, BOTTOM);
|
||||
text("N", circlex, circley - circlediameter/2 - 10);
|
||||
textAlign(CENTER, TOP);
|
||||
text("S", circlex, circley + circlediameter/2 + 10);
|
||||
textAlign(RIGHT, CENTER);
|
||||
text("W", circlex - circlediameter/2 - 10, circley);
|
||||
textAlign(LEFT, CENTER);
|
||||
text("E", circlex + circlediameter/2 + 10, circley);
|
||||
}
|
||||
|
||||
|
||||
void drawAngle(float angle, int circlex, int circley, int circlediameter, String title) {
|
||||
angle = angle + PI/2;
|
||||
|
||||
noStroke();
|
||||
ellipse(circlex, circley, circlediameter, circlediameter);
|
||||
fill(#ff0000);
|
||||
strokeWeight(4);
|
||||
stroke(#ff0000);
|
||||
line(circlex - circlediameter/2 * sin(angle), circley - circlediameter/2 * cos(angle), circlex + circlediameter/2 * sin(angle), circley + circlediameter/2 * cos(angle));
|
||||
noStroke();
|
||||
fill(#ffffff);
|
||||
textAlign(CENTER, BOTTOM);
|
||||
text(title, circlex, circley - circlediameter/2 - 30);
|
||||
}
|
||||
|
||||
void draw() {
|
||||
|
||||
quaternionToYawPitchRoll(q, Euler);
|
||||
|
||||
background(#000000);
|
||||
fill(#ffffff);
|
||||
|
||||
textFont(font, 20);
|
||||
//float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
|
||||
//text("temp:\n" + temp_decoded + " C", 350, 250);
|
||||
textAlign(LEFT, TOP);
|
||||
text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 10);
|
||||
text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " + degrees(Euler[2]), 200, 10);
|
||||
|
||||
drawcompass(Euler[0], VIEW_SIZE_X/2 - 250, VIEW_SIZE_Y/2, 200);
|
||||
drawAngle(Euler[1], VIEW_SIZE_X/2, VIEW_SIZE_Y/2, 200, "Pitch:");
|
||||
drawAngle(Euler[2], VIEW_SIZE_X/2 + 250, VIEW_SIZE_Y/2, 200, "Roll:");
|
||||
}
|
||||
|
||||
|
||||
void quaternionToYawPitchRoll(float [] q, float [] ypr) {
|
||||
float gx, gy, gz; // estimated gravity direction
|
||||
|
||||
gx = 2 * (q[1]*q[3] - q[0]*q[2]);
|
||||
gy = 2 * (q[0]*q[1] + q[2]*q[3]);
|
||||
gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
|
||||
|
||||
ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
|
||||
ypr[1] = atan2(gx, sqrt(gy*gy + gz*gz));
|
||||
ypr[2] = atan2(gy, sqrt(gx*gx + gz*gz));
|
||||
}
|
||||
|
||||
|
||||
+674
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
@@ -0,0 +1,9 @@
|
||||
If you run the attitude estimator by following command, the software will write packets
|
||||
over /dev/ttyS1 which is used for 3D visualization.
|
||||
|
||||
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
|
||||
|
||||
This visualization code can be executed by Processing.
|
||||
Download : http://www.processing.org/download/
|
||||
|
||||
The visualization code works with Processing version 1.5.1.
|
||||
Reference in New Issue
Block a user