mirror of https://github.com/ArduPilot/ardupilot
Line ending property changes.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1649 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
f10cbb011f
commit
a4e791dc92
|
|
@ -1,29 +1,29 @@
|
|||
/*
|
||||
AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
|
||||
Code by James Goppert. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and / or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
*/
|
||||
|
||||
|
||||
#include "AP_Compass_HIL.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
void AP_Compass_HIL::read()
|
||||
{
|
||||
// values set by setHIL function
|
||||
}
|
||||
|
||||
// Update raw magnetometer values from HIL data
|
||||
//
|
||||
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
|
||||
{
|
||||
// TODO: map floats to raw
|
||||
mag_x = _mag_x;
|
||||
mag_y = _mag_y;
|
||||
mag_z = _mag_z;
|
||||
}
|
||||
/*
|
||||
AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
|
||||
Code by James Goppert. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and / or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
*/
|
||||
|
||||
|
||||
#include "AP_Compass_HIL.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
void AP_Compass_HIL::read()
|
||||
{
|
||||
// values set by setHIL function
|
||||
}
|
||||
|
||||
// Update raw magnetometer values from HIL data
|
||||
//
|
||||
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
|
||||
{
|
||||
// TODO: map floats to raw
|
||||
mag_x = _mag_x;
|
||||
mag_y = _mag_y;
|
||||
mag_z = _mag_z;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,16 +1,16 @@
|
|||
#ifndef AP_Compass_HIL_H
|
||||
#define AP_Compass_HIL_H
|
||||
|
||||
#include "Compass.h"
|
||||
|
||||
class AP_Compass_HIL : public Compass
|
||||
{
|
||||
public:
|
||||
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||
|
||||
virtual bool init();
|
||||
virtual void read();
|
||||
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
|
||||
};
|
||||
|
||||
#endif
|
||||
#ifndef AP_Compass_HIL_H
|
||||
#define AP_Compass_HIL_H
|
||||
|
||||
#include "Compass.h"
|
||||
|
||||
class AP_Compass_HIL : public Compass
|
||||
{
|
||||
public:
|
||||
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||
|
||||
virtual bool init();
|
||||
virtual void read();
|
||||
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,141 +1,141 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor is conected to I2C port
|
||||
Sensor is initialized in Continuos mode (10Hz)
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include <math.h>
|
||||
#include "WConstants.h"
|
||||
|
||||
#include <Wire.h>
|
||||
#include "AP_Compass_HMC5843.h"
|
||||
|
||||
#define COMPASS_ADDRESS 0x1E
|
||||
#define ConfigRegA 0x00
|
||||
#define ConfigRegB 0x01
|
||||
#define magGain 0x20
|
||||
#define PositiveBiasConfig 0x11
|
||||
#define NegativeBiasConfig 0x12
|
||||
#define NormalOperation 0x10
|
||||
#define ModeRegister 0x02
|
||||
#define ContinuousConversion 0x00
|
||||
#define SingleConversion 0x01
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
bool AP_Compass_HMC5843::init()
|
||||
{
|
||||
unsigned long currentTime = millis(); // record current time
|
||||
int numAttempts = 0;
|
||||
int success = 0;
|
||||
|
||||
delay(10);
|
||||
|
||||
// calibration initialisation
|
||||
calibration[0] = 1.0;
|
||||
calibration[1] = 1.0;
|
||||
calibration[2] = 1.0;
|
||||
|
||||
while( success == 0 && numAttempts < 5 )
|
||||
{
|
||||
// record number of attempts at initialisation
|
||||
numAttempts++;
|
||||
|
||||
// force positiveBias (compass should return 715 for all channels)
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegA);
|
||||
Wire.send(PositiveBiasConfig);
|
||||
if (0 != Wire.endTransmission())
|
||||
continue; // compass not responding on the bus
|
||||
delay(50);
|
||||
|
||||
// set gains
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegB);
|
||||
Wire.send(magGain);
|
||||
Wire.endTransmission();
|
||||
delay(10);
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ModeRegister);
|
||||
Wire.send(SingleConversion);
|
||||
Wire.endTransmission();
|
||||
delay(10);
|
||||
|
||||
// read values from the compass
|
||||
read();
|
||||
delay(10);
|
||||
|
||||
// calibrate
|
||||
if( abs(mag_x) > 500 && abs(mag_x) < 1000 && abs(mag_y) > 500 && abs(mag_y) < 1000 && abs(mag_z) > 500 && abs(mag_z) < 1000)
|
||||
{
|
||||
calibration[0] = fabs(715.0 / mag_x);
|
||||
calibration[1] = fabs(715.0 / mag_y);
|
||||
calibration[2] = fabs(715.0 / mag_z);
|
||||
|
||||
// mark success
|
||||
success = 1;
|
||||
}
|
||||
|
||||
// leave test mode
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegA);
|
||||
Wire.send(NormalOperation);
|
||||
Wire.endTransmission();
|
||||
delay(50);
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ModeRegister);
|
||||
Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz)
|
||||
Wire.endTransmission(); // End transmission
|
||||
delay(50);
|
||||
}
|
||||
return(success);
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
void AP_Compass_HMC5843::read()
|
||||
{
|
||||
int i = 0;
|
||||
byte buff[6];
|
||||
Vector3f rot_mag;
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(0x03); //sends address to read from
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
//Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device
|
||||
while(Wire.available())
|
||||
{
|
||||
buff[i] = Wire.receive(); // receive one byte
|
||||
i++;
|
||||
}
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
if (i==6) // All bytes received?
|
||||
{
|
||||
// MSB byte first, then LSB, X,Y,Z
|
||||
mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis
|
||||
mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
|
||||
mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
|
||||
last_update = millis(); // record time of update
|
||||
// rotate and offset the magnetometer values
|
||||
// XXX this could well be done in common code...
|
||||
rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
|
||||
rot_mag = rot_mag + _offset.get();
|
||||
mag_x = rot_mag.x;
|
||||
mag_y = rot_mag.y;
|
||||
mag_z = rot_mag.z;
|
||||
}
|
||||
}
|
||||
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor is conected to I2C port
|
||||
Sensor is initialized in Continuos mode (10Hz)
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include <math.h>
|
||||
#include "WConstants.h"
|
||||
|
||||
#include <Wire.h>
|
||||
#include "AP_Compass_HMC5843.h"
|
||||
|
||||
#define COMPASS_ADDRESS 0x1E
|
||||
#define ConfigRegA 0x00
|
||||
#define ConfigRegB 0x01
|
||||
#define magGain 0x20
|
||||
#define PositiveBiasConfig 0x11
|
||||
#define NegativeBiasConfig 0x12
|
||||
#define NormalOperation 0x10
|
||||
#define ModeRegister 0x02
|
||||
#define ContinuousConversion 0x00
|
||||
#define SingleConversion 0x01
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
bool AP_Compass_HMC5843::init()
|
||||
{
|
||||
unsigned long currentTime = millis(); // record current time
|
||||
int numAttempts = 0;
|
||||
int success = 0;
|
||||
|
||||
delay(10);
|
||||
|
||||
// calibration initialisation
|
||||
calibration[0] = 1.0;
|
||||
calibration[1] = 1.0;
|
||||
calibration[2] = 1.0;
|
||||
|
||||
while( success == 0 && numAttempts < 5 )
|
||||
{
|
||||
// record number of attempts at initialisation
|
||||
numAttempts++;
|
||||
|
||||
// force positiveBias (compass should return 715 for all channels)
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegA);
|
||||
Wire.send(PositiveBiasConfig);
|
||||
if (0 != Wire.endTransmission())
|
||||
continue; // compass not responding on the bus
|
||||
delay(50);
|
||||
|
||||
// set gains
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegB);
|
||||
Wire.send(magGain);
|
||||
Wire.endTransmission();
|
||||
delay(10);
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ModeRegister);
|
||||
Wire.send(SingleConversion);
|
||||
Wire.endTransmission();
|
||||
delay(10);
|
||||
|
||||
// read values from the compass
|
||||
read();
|
||||
delay(10);
|
||||
|
||||
// calibrate
|
||||
if( abs(mag_x) > 500 && abs(mag_x) < 1000 && abs(mag_y) > 500 && abs(mag_y) < 1000 && abs(mag_z) > 500 && abs(mag_z) < 1000)
|
||||
{
|
||||
calibration[0] = fabs(715.0 / mag_x);
|
||||
calibration[1] = fabs(715.0 / mag_y);
|
||||
calibration[2] = fabs(715.0 / mag_z);
|
||||
|
||||
// mark success
|
||||
success = 1;
|
||||
}
|
||||
|
||||
// leave test mode
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ConfigRegA);
|
||||
Wire.send(NormalOperation);
|
||||
Wire.endTransmission();
|
||||
delay(50);
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(ModeRegister);
|
||||
Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz)
|
||||
Wire.endTransmission(); // End transmission
|
||||
delay(50);
|
||||
}
|
||||
return(success);
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
void AP_Compass_HMC5843::read()
|
||||
{
|
||||
int i = 0;
|
||||
byte buff[6];
|
||||
Vector3f rot_mag;
|
||||
|
||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.send(0x03); //sends address to read from
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
//Wire.beginTransmission(COMPASS_ADDRESS);
|
||||
Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device
|
||||
while(Wire.available())
|
||||
{
|
||||
buff[i] = Wire.receive(); // receive one byte
|
||||
i++;
|
||||
}
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
if (i==6) // All bytes received?
|
||||
{
|
||||
// MSB byte first, then LSB, X,Y,Z
|
||||
mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis
|
||||
mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
|
||||
mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
|
||||
last_update = millis(); // record time of update
|
||||
// rotate and offset the magnetometer values
|
||||
// XXX this could well be done in common code...
|
||||
rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
|
||||
rot_mag = rot_mag + _offset.get();
|
||||
mag_x = rot_mag.x;
|
||||
mag_y = rot_mag.y;
|
||||
mag_z = rot_mag.z;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,54 +1,54 @@
|
|||
#ifndef AP_Compass_HMC5843_H
|
||||
#define AP_Compass_HMC5843_H
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
|
||||
#include "Compass.h"
|
||||
|
||||
// orientations for DIYDrones magnetometer
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
|
||||
|
||||
// orientations for Sparkfun magnetometer
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
|
||||
|
||||
class AP_Compass_HMC5843 : public Compass
|
||||
{
|
||||
private:
|
||||
float calibration[3];
|
||||
public:
|
||||
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||
virtual bool init();
|
||||
virtual void read();
|
||||
};
|
||||
#endif
|
||||
#ifndef AP_Compass_HMC5843_H
|
||||
#define AP_Compass_HMC5843_H
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
|
||||
#include "Compass.h"
|
||||
|
||||
// orientations for DIYDrones magnetometer
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
|
||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
|
||||
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
|
||||
|
||||
// orientations for Sparkfun magnetometer
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180
|
||||
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
|
||||
|
||||
class AP_Compass_HMC5843 : public Compass
|
||||
{
|
||||
private:
|
||||
float calibration[3];
|
||||
public:
|
||||
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||
virtual bool init();
|
||||
virtual void read();
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,121 +1,121 @@
|
|||
#ifndef Compass_h
|
||||
#define Compass_h
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
|
||||
// standard rotation matrices
|
||||
#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
|
||||
#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
|
||||
|
||||
class Compass
|
||||
{
|
||||
public:
|
||||
int mag_x; ///< magnetic field strength along the X axis
|
||||
int mag_y; ///< magnetic field strength along the Y axis
|
||||
int mag_z; ///< magnetic field strength along the Z axis
|
||||
float heading; ///< compass heading in radians
|
||||
float heading_x; ///< compass vector X magnitude
|
||||
float heading_y; ///< compass vector Y magnitude
|
||||
unsigned long last_update; ///< millis() time of last update
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// @param key Storage key used for configuration data.
|
||||
///
|
||||
Compass(AP_Var::Key key);
|
||||
|
||||
/// Initialize the compass device.
|
||||
///
|
||||
/// @returns True if the compass was initialized OK, false if it was not
|
||||
/// found or is not functioning.
|
||||
///
|
||||
virtual bool init();
|
||||
|
||||
/// Read the compass and update the mag_ variables.
|
||||
///
|
||||
virtual void read() = 0;
|
||||
|
||||
/// Calculate the tilt-compensated heading_ variables.
|
||||
///
|
||||
/// @param roll The current airframe roll angle.
|
||||
/// @param pitch The current airframe pitch angle.
|
||||
///
|
||||
virtual void calculate(float roll, float pitch);
|
||||
|
||||
/// Set the compass orientation matrix, used to correct for
|
||||
/// various compass mounting positions.
|
||||
///
|
||||
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
|
||||
/// to the body frame.
|
||||
///
|
||||
virtual void set_orientation(const Matrix3f &rotation_matrix);
|
||||
|
||||
/// Sets the compass offset x/y/z values.
|
||||
///
|
||||
/// @param offsets Offsets to the raw mag_ values.
|
||||
///
|
||||
virtual void set_offsets(const Vector3f &offsets);
|
||||
|
||||
/// Saves the current compass offset x/y/z values.
|
||||
///
|
||||
/// This should be invoked periodically to save the offset values maintained by
|
||||
/// ::null_offsets.
|
||||
///
|
||||
virtual void save_offsets();
|
||||
|
||||
/// Returns the current offset values
|
||||
///
|
||||
/// @returns The current compass offsets.
|
||||
///
|
||||
virtual Vector3f &get_offsets();
|
||||
|
||||
/// Program new offset values.
|
||||
///
|
||||
/// XXX DEPRECATED
|
||||
///
|
||||
/// @param x Offset to the raw mag_x value.
|
||||
/// @param y Offset to the raw mag_y value.
|
||||
/// @param z Offset to the raw mag_z value.
|
||||
///
|
||||
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
|
||||
|
||||
/// Perform automatic offset updates using the results of the DCM matrix.
|
||||
///
|
||||
/// @param dcm_matrix The DCM result matrix.
|
||||
///
|
||||
void null_offsets(const Matrix3f &dcm_matrix);
|
||||
|
||||
|
||||
/// Sets the local magnetic field declination.
|
||||
///
|
||||
/// @param radians Local field declination.
|
||||
///
|
||||
virtual void set_declination(float radians);
|
||||
|
||||
protected:
|
||||
AP_Var_group _group; ///< storage group holding the compass' calibration data
|
||||
AP_VarS<Matrix3f> _orientation_matrix;
|
||||
AP_VarS<Vector3f> _offset;
|
||||
AP_Float _declination;
|
||||
|
||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
|
||||
Vector3f _mag_body_last; ///< ?? used by offset nulling
|
||||
};
|
||||
#endif
|
||||
#ifndef Compass_h
|
||||
#define Compass_h
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
|
||||
// standard rotation matrices
|
||||
#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
|
||||
#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
|
||||
#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
|
||||
#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
|
||||
#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
|
||||
|
||||
class Compass
|
||||
{
|
||||
public:
|
||||
int mag_x; ///< magnetic field strength along the X axis
|
||||
int mag_y; ///< magnetic field strength along the Y axis
|
||||
int mag_z; ///< magnetic field strength along the Z axis
|
||||
float heading; ///< compass heading in radians
|
||||
float heading_x; ///< compass vector X magnitude
|
||||
float heading_y; ///< compass vector Y magnitude
|
||||
unsigned long last_update; ///< millis() time of last update
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// @param key Storage key used for configuration data.
|
||||
///
|
||||
Compass(AP_Var::Key key);
|
||||
|
||||
/// Initialize the compass device.
|
||||
///
|
||||
/// @returns True if the compass was initialized OK, false if it was not
|
||||
/// found or is not functioning.
|
||||
///
|
||||
virtual bool init();
|
||||
|
||||
/// Read the compass and update the mag_ variables.
|
||||
///
|
||||
virtual void read() = 0;
|
||||
|
||||
/// Calculate the tilt-compensated heading_ variables.
|
||||
///
|
||||
/// @param roll The current airframe roll angle.
|
||||
/// @param pitch The current airframe pitch angle.
|
||||
///
|
||||
virtual void calculate(float roll, float pitch);
|
||||
|
||||
/// Set the compass orientation matrix, used to correct for
|
||||
/// various compass mounting positions.
|
||||
///
|
||||
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
|
||||
/// to the body frame.
|
||||
///
|
||||
virtual void set_orientation(const Matrix3f &rotation_matrix);
|
||||
|
||||
/// Sets the compass offset x/y/z values.
|
||||
///
|
||||
/// @param offsets Offsets to the raw mag_ values.
|
||||
///
|
||||
virtual void set_offsets(const Vector3f &offsets);
|
||||
|
||||
/// Saves the current compass offset x/y/z values.
|
||||
///
|
||||
/// This should be invoked periodically to save the offset values maintained by
|
||||
/// ::null_offsets.
|
||||
///
|
||||
virtual void save_offsets();
|
||||
|
||||
/// Returns the current offset values
|
||||
///
|
||||
/// @returns The current compass offsets.
|
||||
///
|
||||
virtual Vector3f &get_offsets();
|
||||
|
||||
/// Program new offset values.
|
||||
///
|
||||
/// XXX DEPRECATED
|
||||
///
|
||||
/// @param x Offset to the raw mag_x value.
|
||||
/// @param y Offset to the raw mag_y value.
|
||||
/// @param z Offset to the raw mag_z value.
|
||||
///
|
||||
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
|
||||
|
||||
/// Perform automatic offset updates using the results of the DCM matrix.
|
||||
///
|
||||
/// @param dcm_matrix The DCM result matrix.
|
||||
///
|
||||
void null_offsets(const Matrix3f &dcm_matrix);
|
||||
|
||||
|
||||
/// Sets the local magnetic field declination.
|
||||
///
|
||||
/// @param radians Local field declination.
|
||||
///
|
||||
virtual void set_declination(float radians);
|
||||
|
||||
protected:
|
||||
AP_Var_group _group; ///< storage group holding the compass' calibration data
|
||||
AP_VarS<Matrix3f> _orientation_matrix;
|
||||
AP_VarS<Vector3f> _offset;
|
||||
AP_Float _declination;
|
||||
|
||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
|
||||
Vector3f _mag_body_last; ///< ?? used by offset nulling
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue