StarFinder
by Max Pix
StarFinder is a... Stars Finder, implemented with Arduino and Stellarium compiled on Nokia N900 with Maemo5
StarFinder implements two commands:
- Point and Click a Star with the Laser Beam, and Stellarium find the name.
- Search a Star name with Stellarium, and find it in the Sky with the Green Laser Beam.
Components
The Sparkfun 9DOF Sensor stick is a small sensor board with 9 degrees of freedom. It includes the ADXL345 accelerometer, the HMC5843 magnetometer, and the ITG-3200 gyro.
It has a I2C bus, and Arduino can communicate with the 3 chips using 3 different I2C Addresses. It is powered with the 3.3Vcc pin of Arduino MiniPro, and data pins does not require any level shifter.
StarFinder use Arduino Pro Mini 3.3V/8Mhz, but it is the new version with ATMEGA328, with 32k Flash: 16K is not enough.
I have choosen the 3.3V version to avoid level-shifters for the sensor board.
The BlueSMiRF Silver is a Sparkfun Bluetooth Module, which implemente the SPP Profile, and is seen a Serial Port from Arduino. This version has a bit less range than BlueSMiRF Gold, and is cheaper. It works as a serial pipe, from 9600 to 115200bps
It can be powered from 3.3V up to 6V for easy battery attachment, and i have used the 3.3V pin of Arduino.
All signal pins are compatible with the UART Pins of Arduino Mini Pro 3.3V.
The MicroLDC 144 is an awesome LCD by 4dsystems. It has 128x128 color resolution, and can be programmed with a simil-C language with an easy IDE, and communicate with Arduino via UART. It has a MicroSHDC slot which can be accessed from the firmware, and contains all the images used by StarFinder. It has 2 GPIO for sound, OneWire, etc. StarFinder use only one pin to synch Arduino with the LCD.
The module can be powered from 4V to 5.5V: it is powered with the raw voltage of 4 AAA rechargeable batteries in StarFinder. Unfortunately, the UART of Arduino is used to connect the BlueTooth receiver, and i used a SPI/UART converter to connect this LCD. I tried to multiplex the UART using a multiplexer/demultiplexer, bu didn't like that solution. The data pins are compatible with 3.3V of Arduino Mini Pro, and no need of level shifters.
This Sparkfun board convert I2C or SPI serial signals to a single-channel UART. Power applied to the VIN pin is sent through a 3.3V LDO regulator before powering the IC. The input pins are 5.5V tolerant, so this board work with both 3.3V and 5V controllers.
I have used it to connect the LCD, because the Arduino UART is used for the Bluetooth receiver. I had tried to use a cheap 4053 multiplexer/demultiplexer, but it required to a more complex software to synch the two channels, and i didn't like that solution.
I hav e slightly modded a cheap legal Green Laser, bought on eBay.
The laser is powered from the same source of Arduin, but a 3.3 voltage regulator with low dropout has been added.
The Laser has been shortened, and two wires added to power it from the voltage regulator.
I have build a small platform to align the sensor on top of the laser. It is made with SINTAFOAM 1:1, a bicomponent polyurethane resin made by prochima.
A glue gun has been used to fix better the platform, and avoid any misalignment between the sensor and the laser.
A button has been used to control the StarFinder functions. The button can be clicked or pushed. I have analyzed the button with OLS Analyzer, to see if there was bouncing, but the analyzer found no multiple clicks.
So, i have decided to use the button clean, with no debouncing hardware.
The data read from the 3 sensor chips from the sparkfun board can't be directly used: the sensors must be calibrated. The magnetometer is the hardest, and the gyroscope easy.
The Magnetometer
A good document for starting is AN3192 Application note by STMicroelectronics
The relationship between the normalized (Mx1, My1, Mz1) and the magnetic sensor RAW measurements (Mx, My, Mz) can be expressed as:
where [M_m] is a 3x3 misalignment matrix between the magnetic sensor sensing axes and the device body axes; M_SCi (i = x, y, z) is the scale factor and M_OSi is the offset caused by hard-iron distortion; [M_si] is a 3x3 matrix caused by soft-iron distortion. The goal of the magnetic sensor calibration is to determine the parameters from MR10 to MR33 so that with any given raw measurements at arbitrary positions, the normalized values can be obtained.
Hard-iron interference magnetic field is normally generated by ferromagnetic materials with permanent magnetic fields that are part of the handheld device structure. These materials could be permanent magnets or magnetized iron or steel. They are time invariant. These unwanted magnetic fields are superimposed on the output of the magnetic sensor measurements of the earth's magnetic field. The effect of this superposition is to bias the magnetic sensor outputs. It is described as M_OSi (i = x, y, z) or MR10, MR20 and MR30
A soft-iron interference magnetic field is generated by the items inside the handheld device. They could be current carrying traces on the PCB or magnetically soft materials. They generate a time varying magnetic field that is superimposed on the magnetic sensor output in response to the earth's magnetic field. The effect of the soft-iron distortion is to make a full round rotation circle become a tilted ellipse. It is described as the [M_si] 3x3 matrix.
Scale factor error is defined as the mismatch of the sensitivity of the magnetic sensor sensing axes. Ideally, the 3-axis magnetic sensors that make up the triad are identical. In reality, however, this may not be the case. Each magnetic sensor channel may have different sensitivities. The effect of the scale factor error causes the full round rotation circle to become an ellipse. It is described as M_SCi (i = x, y, z).
Misalignment error is defined as the angles between the magnetic sensor sensing axes and the device body axes. When assembling the magnetometer in the handheld device, these small angles always exist and need to be compensated. It is described as [M_m] 3x3 matrix.
Magnetic sensor calibration can be performed by 3 full round rotations along with device body axis Zb down, Yb down, and Xb down respectively, at a leveled smooth surface without a nearby interference magnetic field. They are 2D rotations. The rotation speed should be slow in order to collect as many data points as possible. But it does not require constant rotation speed and an accurate sampling time interval. The full round rotation can be clockwise or counterclockwise. Collected magnetic sensor raw data of 3 full round rotations
is used to accurately determine the 12 magnetic sensor calibration parameters.
3D random rotations are performed by rotating the handheld device in random directions. If the handheld device doesn't have hard-iron and soft-iron interference magnetic fields, and the scale factor of each axis is identical, and the Magnetometer magnetic sensor's sensing axes are aligned to the device body axes, then each full round rotation forms a centered circle with the same radius and the 3D rotations form a centered sphere. However, due to the hard-iron and soft-iron magnetic field distortions, and the errors of the scale factor and the misalignment, the centered sphere becomes a shifted, tilted ellipsoid when plotting the collected magnetic sensor raw data.
The magnetic sensor calibration can only compensate the hard-iron and
soft-iron interference magnetic field generated by the handheld device itself. This means that during full round rotations of the calibration, the hard-iron and soft-iron fields also rotate with the device.
The least square fitting ellipsoid method can be used to discover the parameters of M_SCi, M_OSi (i = x, y, z) and [M_si]. The magnetic sensor raw data used here could be three 2D full round rotations, or 3D rotations, or both.
This the plot of about 40.000 points collected to Calibrate the HMC5843 Magnetometer used by StarFinder. This is Scattered Plot made with Dplot.
It is not esay collecting so many points. I have put StarFinder inside a box with numbered sides, and used the Blutooth connection with a PC. I have also Planned on paper all the rotations
The RAW points can be used to determine 6 of the 12 parameters, using the MATLAB code by Sebastian O.H. Madgwick . I think this code should work also with Octavewhich is free.
The Madgwick code calculate only 6 parametersMR10, MR20, MR30 and MR11, MR22, MR33, but it can be enough: for example, the HMC5843 used by StarFinder plotted a good Sphere, and there are not hard/soft iron distorsions.
I have also tried another MATLAB code by Alain Barraud, Suzanne Lesecq. (this original link). This code calculate all the 12 parameters, but unfurtunately MATLAB/OCTAVE crashes if there are too many points, with memory fault.
The Accelerometer
A good document for starting is AN3182 Application note by STMicroelectronics
The relationship between the normalized (Ax1, Ay1, Az1) and the accelerometer RAW measurements (Ax, Ay, Az) can be expressed as:
Where [A_m] is the 3 x 3 misalignment matrix between the accelerometer sensing axes and the device body axes, A_SCi (i = x, y, z) is the sensitivity (or scale factor) and A_OSi is the zero-g level (or offset).
The goal of accelerometer calibration is to determine 12 parameters from ACC10 to ACC33, so that with any given raw measurements at arbitrary positions, the normalized values (Ax1, Ay1, Az1 can be obtained, resulting in:
Calibration can be performed at 6 stationary positions as shown next table. Collect 5 to 10 seconds of accelerometer raw data with ODR = 100 Hz at each position with known (Ax1, Ay1, Az1). Then apply the least square method to obtain the 12 accelerometer calibration parameters.
The data collected from the accelerometer can be used to determine the 12 calibration parameters with the the Least Square Method. The previous equation can be rewritten as:
Y = w * X
where matrix X is the 12 calibration parameters that need to be determined; Matrix w is sensor raw data collected at 6 stationary positions; Matrix Y is the known normalized Earth gravity vector.
The calibration parameter matrix X can be determined by the least square method as:
The problem with this calibration is that the sensor must be perfectly aligned when collecting data, to get a good precision. I have put the sensor on a PCB, and have used a squadra to align it on 90 degrees.
This is a little C program i have written to calculate the matrix X, using the data collected from the accelerometer. It must compiled with alglib. This is the calibration data of StarFinder ADXL345, to test the code. I hope it works for you.
The Gyroscope
I found some documentation int this document of Sebastian O.H. Madgwick
MEMS gyroscopes are subject to bias drift so calibration cannot assume a constant bias. Instead, AHRS and IMU algorithms will typically estimate the gyroscope bias in real-time. Consequently, gyroscope calibration need only be concerned with estimation of the sensor Gains.
I have determined the Gains in the following way for the ITG-3200 :-). The StarFinder was connected wireless to N900 using Bluetooth.
The values i got was 13.92, 14.27, 14.01 
, quite identical to the tipical values in the datasheet: 14.375. I decided to use the values of the datahseet, converted in radians/sec: 14.375 / (PI / 180) = 823.627 
for all the 3 axes.
The Biases can be easily determined with a Zero Calibration, but can change with temperature, and StarFinder execute a Zero Calibration at runtime during startup. The StarFinder collect samples for 3 seconds, and calculate an average for each axe. The cons of Runtime calibration is that the device must be stationary during PowerOn
The AHRS update procedure requires 2 parameters Kp and Ki. One method, called the "no-brain-method" :-) is the Ziegler–Nichols method
In the 1940's, Ziegler and Nichols devised two empirical methods for obtaining controller parameters. [...] With improved optimization software, most manual methods such as these are no longer used. However, even with computer aids, the following two methods are still employed today, and are considered among the most common:
The Ziegler-Nichols method allows you to use the ultimate gain value Ku, and the ultimate period of oscillation Pu, to calculate Kp . It is a simple method of tuning PID controllers and can be refined to give better approximations of the controller. [...]
Determining the ultimate gain value Ku, is accomplished by finding the value of the proportional-only gain that causes the control loop to oscillate indefinitely at steady state. This means that the gain from the Ki is set to zero so that the influence of Kp can be determined. [...] The ultimate period Pu is the time required to complete one full oscillation while the system is at steady state. These two parameters, Ku and Pu, are used to find the loop-tuning constants of the controller PI. To find the values of these parameters, and to calculate the tuning constants, use the following procedure:
- Remove integral action : set Ki to 0 to remove integral gain
- Create a small disturbance in the loop by changing the set point. Adjust Kp, increasing and/or decreasing, until the oscillations have constant amplitude.
- Record the gain value (Ku) and period of oscillation (Pu).
Then you have:
Kp = Ku / 2.2
Ki = 1.2 * Kp / Pu
And the sistem should work like this:
I put Serial.print() into the main loop of the firmware, to send the samples of Yaw to the PC via Bluetooth . Then i plotted the samples with DPlot, and adjusted Kp in the AHRS Update code, until i got the correct value for Ku
Well, i couldn't calc a good value for Pu using the plotted samples. So, i simply started with a very high value for Ki, and gradully reduces the value till a got a stable system, like in the previous image.
StarFinder use Ku = 30, Kp = 13.6, Ki = 0.2
Calibrating Misalignment
TO DO (maybe)
The ULCD144 can be flash with 2 different firmware. I used the Goldelox firmware, to program the device with a nice IDE. The Program receive 3 type of commands from Arduino, to show the Compass, display a message, search a Star.
The Commands are sent from Arduino SPI, but are received by the LCD UART, thanks to the Sparkfun SPI/UART Converter.
The LCD takes some time to execute the graphics commands, and when is ready to get anothere command, pull up the GPIO pin 1, to synch with the Arduino. Arduino CAN'T send more commands to the LCD if it is not ready, or this commands are LOST.
Clipping is available in the graphics commands set of ULCD144, and i have used it to remove flashing of the text Area. When the Compass image is drawed, it is not drawed only once, but 4 times with different clippings, and this way the text area is NOT erased (and does not flash).
During normal operation in mode 1, the LCD show a Compass, the Yaw, Pitch, and a flashing yellow rectangle for the Pitch.
In the SHDC card are saved 365 images, for each Degree of Rotation. This avoid using any Vectorial Graphics.
From Stellarium you can select a Star, and click the Green Laser Button, to find it in the Sky. A message display in the StarFinder LCD in mode 3. Now you click the StarFinder Button, to go in mode 2
In mode 2 StarFinder is in search mode, and the yaw and pitch of the Star are displayed under the yaw/pitch of the Laser.
A flashing Green Rectagle is displayed, showing the Star Position, and StarFinder must be rotated until such Rectagle overlap the Yellow Rectangle, showing the laser orientation.
These are the 360 images saved inside the USHDC
var sb[128];
var n;
var syn[4];
var mode;
var yaw;
var pitch;
var yaws;
var pitchs;
var i;
var incr := 0x0041;
var img;
var m;
var l;
var c;
var pi;
var rad;
var alfa;
var beta;
var a;
var b;
var d;
var cmd_length := 73;
var msg_length := 60;
var msg[60];
func reset()
com_Reset();
com_Init(sb,128,0);
setbaud(BAUD_115200);
endfunc
func main()
pin_Set(OUTPUT,IO1);
pin_LO(IO1);
while(media_Init() == 0);
com_Init(sb,128,0);
setbaud(BAUD_115200);
gfx_Clipping(ON);
// Splash
media_SetSector(0x00, 0x6000);
media_Image(0,0);
pause(3000);
gfx_BGcolour(0x0000);
gfx_Cls();
pin_HI(IO1);
repeat
n := com_Count();
if (n < cmd_length)
pause(1);
continue;
endif
if (n > cmd_length)
reset();
continue;
endif
for (i:=0;i<4;i++) syn[i]:=serin();
if (syn[0]!='S' || syn[1]!='T' || syn[2]!='A' || syn[3] != 'R')
reset();
continue;
endif
pin_LO(IO1);
mode:=serin();
c:=serin();
yaw:=(c<<8)&0xFF00;
c:=serin();
yaw:=yaw | (c&0x00FF);
c:=serin();
pitch:=(c<<8)&0xFF00;
c:=serin();
pitch:=pitch | (c&0x00FF);
c:=serin();
yaws:=(c<<8)&0xFF00;
c:=serin();
yaws:=yaws | (c&0x00FF);
c:=serin();
pitchs:=(c<<8)&0xFF00;
c:=serin();
pitchs:=pitchs | (c&0x00FF);
for (i:=0; i<msg_length; i++) msg[i]:=serin();
if (mode != 3)
img:=0x00;
if (yaw != 359) img := (yaw+1) * incr;
media_SetSector(0x00,img);
gfx_Set(CLIPPING,ON);
gfx_ClipWindow(0,0,127,95);
media_Image(0,0);
gfx_Set(CLIPPING,ON);
gfx_ClipWindow(0,96,41,113);
media_Image(0,0);
gfx_Set(CLIPPING,ON);
gfx_ClipWindow(89,96,127,113);
media_Image(0,0);
gfx_Set(CLIPPING,ON);
gfx_ClipWindow(0,114,127,127);
media_Image(0,0);
gfx_Set(CLIPPING,OFF);
txt_Set(TEXT_OPACITY,OPAQUE);
txt_Set(TEXT_COLOUR,0xF800);
txt_Set(TEXT_HIGHLIGHT,0xFFC0);
gfx_MoveTo(42,96);
putnum(DEC3Z,yaw);
gfx_MoveTo(65,96);
if (pitch >= 0)
putnum(DEC3Z,pitch);
else
putnum(DEC2Z,pitch);
endif
if (mode == 1)
gfx_RectangleFilled(42,106,61,112,0x003F);
gfx_RectangleFilled(64,106,84,112,0x003F);
endif
pi := pitch;
if (pi < 0) pi := 0;
if (pi > 90) pi := 90;
c := pi * 64 / 90;
gfx_RectangleFilled(57,c-1,69,c+1,0xFF80);
endif
if (mode == 2)
txt_Set(TEXT_OPACITY,OPAQUE);
txt_Set(TEXT_COLOUR,0xFFFF);
txt_Set(TEXT_HIGHLIGHT,0x003F);
gfx_MoveTo(42,106);
putnum(DEC3Z,yaws);
gfx_MoveTo(65,106);
if (pitchs >= 0)
putnum(DEC3Z,pitchs);
else
putnum(DEC2Z,pitchs);
endif
pi := pitchs;
if (pi < 0) pi := 0;
if (pi > 90) pi := 90;
c := ((90 - pi) * 64) / 90;
d := 64 - c;
alfa := yaws - yaw;
if (alfa < 0) alfa := alfa + 360;
a := d + (c * (127 - COS(alfa))) / 127;
b := d + (c * (127 + SIN(alfa))) / 127;
gfx_RectangleFilled(b-6,a-1,b+6,a+1,0x0780);
endif
if (mode == 3)
gfx_Cls();
txt_Set(TEXT_OPACITY,TRANSPARENT);
txt_Set(TEXT_COLOUR,0xFFFF);
gfx_MoveTo(5,16);
for (i:=0; msg[i]!=0 && i<16; i++) putch(msg[i]);
gfx_MoveTo(5,26);
for (; msg[i]!=0 && i<32; i++) putch(msg[i]);
gfx_MoveTo(5,36);
for (; msg[i]!=0 && i<48; i++) putch(msg[i]);
gfx_MoveTo(5,46);
for (; msg[i]!=0; i++) putch(msg[i]);
endif
reset();
pin_HI(IO1);
forever
endfunc
class UTIL
float2byte, byte2float are used to send a normalized float (0.123456789) from N900 to Arduino, e viceversa.
writeReg, readFrom, getRaw are used by the 3 drivers of the sensor board, to read a write on the I2C
quat2euler convert to tait bryan angles the quaternion updated by the AHRS Code
quat2vec convert to (x,y,z) Stellarium Rectagle Coordinates the quaternion updated by the AHRS Code
vec2euler convert (x,y,z) Stellarium Rectagle Coordinates to Tait Bryan Angles
rotx, roty, rotz rotate a (x,y,z) vector, and are used to add a (yaw,pitch) offset to the (x,y,z) Orientation on the Laser.
class SpiUartDevice is the driver of SPI/UART converter
class MEMS, HMC5843, ITG3200, ADXL345 are the drivers of the 3 sensor chips. MEMS is a base class for the other 3 classes.
class AHRS implements the Sebastian O.H. Madgwick AHRS Fusion code
class ULCD144 is the driver of the LCD
class StarFinderButton is the driver of button, and can manage click and push
class StarFinder is the App. It is status driven, and works with a main loop and no interrupts. The several Stats implements the 2 main functions of StarFinder, and a Calibration Menu. The calibration here is NOT the calibration of sensor, BUT only the calc of yaw/pitch offsets for Star Positions.
These offset are saved in EEPROM, and are added to the values Euler/Vec returned by the sensor, to get a better orientation. Well, really i DON'T know why there are these drifts from real (from Stellarium) Stars orientation. I think that the sensor calibration is good, and the Fusion ARHS code too. I can only be pragmatic, and add this code to calculate the offsets and get a better precision.
StarFinder can calculate and save in EEPROM the offsets of several regions of the Sky, using 13 Calibration Bright Stars for reference. At runtime, StarFinder every 300ms choose the offsets of the region near to the laser orientation.
#include <SPI.h>
#include <OneWire.h>
#include <Wire.h>
//-----------------------------------------------------------------------------------------------------------------------------------
class UTIL
{
public:
inline static void float2byte(float f, byte& b3, byte& b2, byte& b1, byte& b0)
{
long pl = (long) (f * 1000000000.0);
unsigned long p = (unsigned long) pl;
b3 = (byte) ((p>>24) & 0x000000FF);
b2 = (byte) ((p>>16) & 0x000000FF);
b1 = (byte) ((p>> 8) & 0x000000FF);
b0 = (byte) ((p ) & 0x000000FF);
}
inline static float byte2float(byte b3, byte b2, byte b1, byte b0)
{
long a =
(((unsigned long)b3<<24) & 0xFF000000) |
(((unsigned long)b2<<16) & 0x00FF0000) |
(((unsigned long)b1<< 8) & 0x0000FF00) |
(((unsigned long)b0 ) & 0x000000FF) ;
float v = (float) ( (float)a / (float)1000000000.0 );
return v;
}
/*
* Angle from y1 to y2, positive CW
* y1 and y2 in [0,359]
*/
inline static float yaw_between(float y1, float y2)
{
if (y1 == 360) y1 = 0;
if (y2 == 360) y2 = 0;
if (y2 - y1 > 180) return y2 - y1 - 360;
else if (y2 - y1 < -180) return 360 - y1 + y2;
else return y2 - y1;
}
inline static int yaw_between(int y1, int y2)
{
return (int) yaw_between((float)y1, (float)y2);
}
inline static void long2char(long n,char t[])
{
char s[10];
char* ps = s;
char* pt = t;
int sign=1;
if (n==0) {*ps=0;ps++;}
if (n<0) {sign=-1;n=-n;}
for (;n>0;)
{
*ps = n % 10;
ps++;
n /= 10;
}
*ps=0;
--ps;
if (sign == -1) {*pt='-';++pt;}
for (;;)
{
*pt=(*ps + '0');
++pt;
if (ps - s == 0) break;
--ps;
}
*pt=0;
}
inline static float Rad2Deg(float rad) { return rad * (180.0 / M_PI); }
inline static float Deg2Rad(float deg) { return deg * (M_PI / 180.0); }
inline static void writeReg(byte dev, byte reg, byte val)
{
Wire.beginTransmission(dev);
Wire.send(reg);
Wire.send(val);
Wire.endTransmission();
}
inline static bool readFrom(byte dev, byte addr, int num, byte _buff[])
{
Wire.beginTransmission(dev);
Wire.send(addr);
Wire.endTransmission();
Wire.beginTransmission(dev);
Wire.requestFrom((int)dev,num);
int i = 0;
while(Wire.available())
{
_buff[i] = Wire.receive();
i++;
}
Wire.endTransmission();
return (i != num);
}
inline static bool getRaw(byte dev,bool LSB_first, byte addr, int* b, int nbytes=6,int nval=3)
{
// extra buffer for security
byte _buf[12];
if (readFrom(dev,addr,nbytes,_buf)) return true;
for (int i=0; i<nval; ++i) b[i] = (int) ((_buf[ (LSB_first?1:0) + i*2 ] << 8) | _buf[ (LSB_first?0:1) +i*2 ]);
return false;
}
// Euler 2 1 3 (Tait-Bryan angles)
inline static void quat2euler(float qw, float qx, float qy, float qz, float* yaw, float* pitch, float* roll)
{
float fpitch,froll,fyaw;
float sqw = qw * qw;
float sqx = qx * qx;
float sqy = qy * qy;
float sqz = qz * qz;
froll = atan2( -2*qx*qz + 2*qw*qy , sqz - sqy - sqx + sqw );
fpitch = asin( 2*qy*qz + 2*qw*qx );
fyaw = atan2( -2*qx*qy + 2*qw*qz , sqy - sqz + sqw - sqx );
*roll = Rad2Deg(froll);
*pitch = Rad2Deg(fpitch);
*yaw = Rad2Deg(fyaw);
*yaw += 270;
if (*yaw > 360) *yaw -= 360;
*yaw = 360.0 - *yaw;
// normalize yaw in [0,359],
// to index the 360 Compass images in usdhc
if (*yaw < 0) *yaw += 360;
if (*yaw >= 360) *yaw -= 360;
}
// Euler 2 1 3 (Tait-Bryan angles)
inline static void quat2euler(float qw, float qx, float qy, float qz, int* yaw, int* pitch, int* roll)
{
float fyaw,fpitch,froll;
quat2euler(qw,qx,qy,qz, &fyaw,&fpitch,&froll);
*yaw = (int)round(fyaw);
*pitch = (int)round(fpitch);
*roll = (int)round(froll);
if (*yaw < 0) *yaw += 360;
if (*yaw >= 360) *yaw -= 360;
}
inline static void normalise(float* x, float* y, float* z)
{
float h = sqrt( (*x)*(*x) + (*y)*(*y) + (*z)*(*z) );
*x /= h;
*y /= h;
*z /= h;
}
// Quat to Direction Vector
inline static void quat2vec(float qw, float qx, float qy, float qz, float* vx, float* vy, float* vz)
{
float c = 2*qy*qz + 2*qw*qx;
float cc = c*c;
float rr = 1 - cc;
float a = (qy*qy - qz*qz + qw*qw - qx*qx);
float aa = a*a;
float b = -2*qx*qy + 2*qw*qz;
float bb = b*b;
*vx = - rr * b;
*vy = rr * a;
*vz = sqrt(rr * (aa + bb)) * c;
normalise(vx,vy,vz);
}
inline static void rotx(float& x, float& y, float& z, float deg)
{
float rad = deg * M_PI / 180.0;
float yr = y * cos(rad) - z * sin(rad);
float zr = y * sin(rad) + z * cos(rad);
y = yr;
z = zr;
}
inline static void roty(float& x, float& y, float& z, float deg)
{
float rad = deg * M_PI / 180.0;
float zr = z * cos(rad) - x * sin(rad);
float xr = z * sin(rad) + x * cos(rad);
z = zr;
x = xr;
}
inline static void rotz(float& x, float& y, float& z, float deg)
{
float rad = deg * M_PI / 180.0;
float xr = x * cos(rad) - y * sin(rad);
float yr = x * sin(rad) + y * cos(rad);
y = yr;
x = xr;
}
inline static void vec2euler(float vx, float vy, float vz, float& yaw, float& pitch)
{
float r = sqrt( vx*vx + vy*vy + vz*vz );
pitch = asin(vz / r);
yaw = atan2(vy,vx);
yaw = 3.0 * M_PI - yaw;
yaw = (short) ( yaw * 180.0 / M_PI );
pitch = (short) ( pitch * 180.0 / M_PI );
if (yaw < 0) yaw += 360;
if (yaw > 360) yaw -= 360;
}
};
//-----------------------------------------------------------------------------------------------------------------------------------
class SpiUartDevice
{
public:
static const int THR = (0x00 << 3);
static const int RHR = (0x00 << 3);
static const int FCR = (0x02 << 3);
static const int LCR = (0x03 << 3);
static const int SPR = (0x07 << 3);
static const int TXLVL = (0x08 << 3);
static const int RXLVL = (0x09 << 3);
static const int IODIR = (0x0A << 3);
static const int IOSTATE = (0x0B << 3);
static const int DLL = (0x00 << 3);
static const int DLH = (0x01 << 3);
static const int EFR = (0x02 << 3);
inline SpiUartDevice() {}
inline void init(unsigned long baudrate = 115200UL, byte sp = 10)
{
selectPin = sp;
// Set baudrate
writeRegister(LCR,0x80);
writeRegister(DLL,0x08);
writeRegister(DLH,0x00);
writeRegister(LCR,0xBF); // access EFR register
writeRegister(EFR,0x10); // enable enhanced functions
writeRegister(LCR,0x03); // 8 data bit, 1 stop bit, no parity
writeRegister(FCR,0x06); // reset TXFIFO, reset RXFIFO, non FIFO mode
writeRegister(FCR,0x01); // enable FIFO mode
}
/*
* Get the number of bytes (characters) available for reading.
* This is data that's already arrived and stored in the receive buffer (which holds 64 bytes).
*/
inline byte available() { return readRegister(RXLVL); }
// Read byte from UART. Returns byte read or or -1 if no data available. Acts in the same manner as 'Serial.read()'
inline int read()
{
if (!available()) return -1;
return readRegister(RHR);
}
// Write byte to UART.
void write(byte value)
{
while (readRegister(TXLVL) == 0) ; // Wait for space in TX buffer
writeRegister(THR, value);
}
inline void flush() { while(available() > 0) read(); }
inline void ioSetDirection(unsigned char bits) { writeRegister(IODIR, bits); }
inline void ioSetState(unsigned char bits) { writeRegister(IOSTATE, bits); }
private:
byte selectPin;
inline void select() { digitalWrite(selectPin,LOW); }
inline void deselect() { digitalWrite(selectPin,HIGH); }
// Write <data> byte to the SC16IS750 register <registerAddress>.
inline void writeRegister(byte registerAddress, byte data)
{
select();
SPI.transfer(registerAddress);
SPI.transfer(data);
deselect();
}
// Read byte from SC16IS750 register at <registerAddress>.
byte readRegister(byte registerAddress)
{
select();
SPI.transfer(0x80 | registerAddress);
char result = SPI.transfer(0xFF);
deselect();
return result;
}
// Check that UART is connected and operational. Perform read/write test to check if UART is working
boolean uartConnected()
{
writeRegister(SPR,(const char)'H');
return ( readRegister(SPR) == (const char)'H' );
}
};
//-----------------------------------------------------------------------------------------------------------------------------------
class MEMS
{
public:
float x,y,z;
inline MEMS() : x(0), y(0), z(0) {}
virtual bool data_available() = 0;
virtual bool get_cal(float* c) = 0;
inline bool get_xyz(float* c)
{
// if some data are non consumed, return that data
if (data_available() && ! get_cal(c))
{
x = c[0];
y = c[1];
z = c[2];
return true;
}
else
{
c[0] = x;
c[1] = y;
c[2] = z;
return false;
}
}
};
//-----------------------------------------------------------------------------------------------------------------------------------
class ADXL345 : public MEMS
{
public:
static const byte ADXL345_POWER_CTL = 0x2d;
static const byte ADXL345_BW_RATE = 0x2c;
static const byte ADXL345_BW_100 = 0x0B;
static const byte ADXL345_BW_50 = 0x0A;
static const byte ADXL345_BW_25 = 0x09;
static const byte ADXL345_DATAX0 = 0x32;
static const byte ADXL345_DATA_FORMAT = 0x31;
static const byte ADXL345_INT_SOURCE = 0x30;
static const byte ADXL345_INT_WATERMARK_BIT = 0x01;
static const byte ADXL345_FIFO_CTL = 0x38;
static const byte ADXL345_FIFO_STATUS = 0x39;
static const byte ADXL345_ADDR = 0x53;
static float acc_cal[4][3];
float x,y,z;
inline ADXL345() {}
virtual inline bool data_available()
{
byte _b;
if (UTIL::readFrom(ADXL345_ADDR, ADXL345_INT_SOURCE,1,&_b)) return false;
return ( (_b >> ADXL345_INT_WATERMARK_BIT) & 1 );
}
virtual inline bool get_cal(float* c)
{
int b[3];
if (UTIL::getRaw(ADXL345_ADDR, true, ADXL345_DATAX0, b)) return true;
c[0] = b[0]*acc_cal[0][0] + b[1]*acc_cal[1][0] + b[2]*acc_cal[2][0] + acc_cal[3][0];
c[1] = b[0]*acc_cal[0][1] + b[1]*acc_cal[1][1] + b[2]*acc_cal[2][1] + acc_cal[3][1];
c[2] = b[0]*acc_cal[0][2] + b[1]*acc_cal[1][2] + b[2]*acc_cal[2][2] + acc_cal[3][2];
return false;
}
inline void init()
{
set_bw50();
set_range2();
set_fifo_stream_none();
delay(100);
power_on();
delay(100);
}
private:
inline void power_on() { UTIL::writeReg(ADXL345_ADDR,ADXL345_POWER_CTL,8); }
inline void set_bw50() { UTIL::writeReg(ADXL345_ADDR,ADXL345_BW_RATE, ADXL345_BW_50); }
inline void set_range2() { UTIL::writeReg(ADXL345_ADDR,ADXL345_DATA_FORMAT, B00000000); }
inline void set_fifo_stream_none() { UTIL::writeReg(ADXL345_ADDR,ADXL345_FIFO_CTL,B10000001); }
};
float ADXL345::acc_cal[4][3] =
{
{ 0.0037969816 , -0.0000692117 , 0.0000110374} ,
{ 0.0000902890 , 0.0038063925 , -0.0000289112} ,
{-0.0000184737 , 0.0000236485 , 0.0039825112} ,
{-0.0654528294 , 0.0123378171 , 0.1142118850}
};
//-----------------------------------------------------------------------------------------------------------------------------------
class HMC5843 : public MEMS
{
public:
static const byte HMC5843_ADDR = 0x1E;
static const byte HMC5843_R_MODE = 2;
static const byte HMC5843_R_CONFA = 0;
static const byte HMC5843_R_CONFB = 1;
static const byte HMC5843_R_XM = 3;
static const byte HMC5843_R_STATUS = 9;
static double mag_cal_c[3];
static double mag_cal_U[3][3];
inline HMC5843() {}
inline void init()
{
// set rate 50Hz
UTIL::writeReg(HMC5843_ADDR,HMC5843_R_CONFA,B00011000);
// set range 1Ga (default)
UTIL::writeReg(HMC5843_ADDR,HMC5843_R_CONFB,B00100000);
// set continuos mode
UTIL::writeReg(HMC5843_ADDR,HMC5843_R_MODE, B00000000);
delay(100);
}
virtual inline bool data_available()
{
byte _b;
if (UTIL::readFrom(HMC5843_ADDR, HMC5843_R_STATUS,1,&_b)) return false;
return ( (_b & B00000001) == 1 );
}
virtual inline bool get_cal(float* c)
{
int b[3];
if (UTIL::getRaw(HMC5843_ADDR,false, HMC5843_R_XM, b)) return true;
c[0] = b[0] * mag_cal_U[0][0] - mag_cal_c[0];
c[1] = b[1] * mag_cal_U[1][1] - mag_cal_c[1];
c[2] = b[2] * mag_cal_U[2][2] - mag_cal_c[2];
return false;
}
private:
};
double HMC5843::mag_cal_c[3] =
{ 0.0508407434, 0.1193248948 ,-0.0166381769 };
double HMC5843::mag_cal_U[3][3] =
{
{ 0.0017885686, 0.0000000000, 0.0000000000 },
{ 0.0000000000, 0.0017508951, 0.0000000000 },
{ 0.0000000000, 0.0000000000, 0.0018910077 },
};
//-----------------------------------------------------------------------------------------------------------------------------------
class ITG3200 : public MEMS
{
public:
static const byte ITG3200_SMPLRT_DIV = 0x15;
static const byte ITG3200_DLPF_FS = 0x16;
static const byte ITG3200_PWR_MGM = 0x3E;
static const byte ITG3200_INT_CFG = 0x17;
static const byte ITG3200_INT_CFG_STATUS = 0x1A;
static const byte ITG3200_GYRO_XOUT = 0x1D;
static const byte ITG3200_TEMP_OUT = 0x1B;
static const byte ITG3200_ADDR = 0x68;
static double gyro_zero_bias[3];
static double gyro_gain[3];
inline ITG3200() {}
inline void init()
{
byte b[6];
UTIL::writeReg(ITG3200_ADDR,ITG3200_SMPLRT_DIV,0);
UTIL::writeReg(ITG3200_ADDR,ITG3200_DLPF_FS, B00011000);
UTIL::writeReg(ITG3200_ADDR,ITG3200_PWR_MGM, B00000000);
UTIL::writeReg(ITG3200_ADDR,ITG3200_INT_CFG, B00000000);
}
virtual inline bool data_available()
{
byte b[6];
UTIL::readFrom(ITG3200_ADDR, ITG3200_INT_CFG_STATUS, 1,&b[0]);
return ( (b[0]&0x01) == 1 );
}
virtual inline bool get_cal(float* c)
{
int b[3];
if (UTIL::getRaw(ITG3200_ADDR,false, ITG3200_GYRO_XOUT, b)) return true;
c[0] = (b[0] - gyro_zero_bias[0]) / gyro_gain[0];
c[1] = (b[1] - gyro_zero_bias[1]) / gyro_gain[1];
c[2] = (b[2] - gyro_zero_bias[2]) / gyro_gain[2];
return false;
}
inline bool get_temp(float* t)
{
int b[3];
if (UTIL::getRaw(ITG3200_ADDR,false, ITG3200_TEMP_OUT, b, 2,1)) return true;
*t = 35 + (((b[0]<<8)|b[1]) + 13200) / 280.0;
return false;
}
inline void zero_cal()
{
float x,y,z;
x = y = z = 0;
int j = 0;
for (int i=0; i<100; ++i)
{
int gd[3];
if (! UTIL::getRaw(ITG3200_ADDR,false, ITG3200_GYRO_XOUT, gd))
{
x += gd[0];
y += gd[1];
z += gd[2];
++j;
}
delay(30);
}
gyro_zero_bias[0] = (float)x / (float)j;
gyro_zero_bias[1] = (float)y / (float)j;
gyro_zero_bias[2] = (float)z / (float)j;
}
private:
};
double ITG3200::gyro_zero_bias[3] = { 49.084795, 4.473684, 115.885965 }; // {0,0,0};
double ITG3200::gyro_gain[3] = { 823.627, 823.627, 823.627 }; // { 13.92, 14.27, 14.01 in gradi }; 14.375 tipico
//-----------------------------------------------------------------------------------------------------------------------------------
/*
* Ku = 30 : max value for Kp = Ku / 2.2 = 13.6
* Ki = 0.2
*/
class AHRS
{
public:
// quaternion elements representing the estimated orientation
float q0;
float q1;
float q2;
float q3;
inline AHRS(float cKpAcc = 13.6, float cKiAcc = 0.2, float cKpMag = 13.6, float cKiMag = 0.2)
: KpAcc(cKpAcc), KiAcc(cKiAcc), KpMag(cKpMag), KiMag(cKiMag), q0(1), q1(0), q2(0), q3(0), exInt(0), eyInt(0), ezInt(0) {}
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float halfT, bool normalize = true);
private:
// proportional gain governs rate of convergence to accelerometer/magnetometer
float KpAcc;
float KpMag;
//integral gain governs rate of convergence of gyroscope biases
float KiAcc;
float KiMag;
//scaled integral error
float exInt;
float eyInt;
float ezInt;
};
void AHRS::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float halfT, bool normalize)
{
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float q0i, q1i, q2i, q3i;
float exAcc, eyAcc, ezAcc;
float exMag, eyMag, ezMag;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// compute reference direction of flux
hx = 2*mx*( 0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
exAcc = (ay*vz - az*vy);
eyAcc = (az*vx - ax*vz);
ezAcc = (ax*vy - ay*vx);
exMag = (my*wz - mz*wy);
eyMag = (mz*wx - mx*wz);
ezMag = (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + exAcc*KiAcc + exMag*KiMag;
eyInt = eyInt + eyAcc*KiAcc + eyMag*KiMag;
ezInt = ezInt + ezAcc*KiAcc + ezMag*KiMag;
// adjusted gyroscope measurements
gx = gx + exAcc*KpAcc + exMag*KpMag + exInt;
gy = gy + eyAcc*KpAcc + eyMag*KpMag + eyInt;
gz = gz + ezAcc*KpAcc + ezMag*KpMag + ezInt;
// integrate quaternion rate and normalise
q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
q0 += q0i;
q1 += q1i;
q2 += q2i;
q3 += q3i;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//-----------------------------------------------------------------------------------------------------------------------------------
class ULCD144
{
public:
inline ULCD144() {}
inline void init(byte p, SpiUartDevice* su)
{
pinMode(p,INPUT);
pin = p;
spi_uart = su;
}
inline void compass(int yaw, int pitch) { cmd(1,yaw,pitch,0,0,""); }
inline void find(int yaw, int pitch, int yaws, int pitchs) { cmd(2,yaw,pitch,yaws,pitchs,""); }
inline void message(char* msg) { cmd(3,0,0,0,0,msg); }
inline bool avail() { return (digitalRead(pin) == HIGH); }
private:
byte pin;
SpiUartDevice* spi_uart;
inline void cmd(int mode, int yaw, int pitch, int yaws, int pitchs, char* msg)
{
// STAR 1,2,3
spi_uart->write((byte)0x53);
spi_uart->write((byte)0x54);
spi_uart->write((byte)0x41);
spi_uart->write((byte)0x52);
spi_uart->write((byte)mode);
// yaw
spi_uart->write( (byte) ((yaw>>8)&0x00FF) );
spi_uart->write( (byte) (yaw&0x00FF) );
// pitch
spi_uart->write( (byte) ((pitch>>8)&0x00FF) );
spi_uart->write( (byte) (pitch&0x00FF) );
// yaws
spi_uart->write( (byte) ((yaws>>8)&0x00FF) );
spi_uart->write( (byte) (yaws&0x00FF) );
// pitchs
spi_uart->write( (byte) ((pitchs>>8)&0x00FF) );
spi_uart->write( (byte) (pitchs&0x00FF) );
// exactly 60 chars, with 0's filling
int i;
for (i=0; i<59 && msg[i]!=0; ++i) spi_uart->write( (byte) msg[i] );
for (; i<60; ++i) spi_uart->write((byte)0x00);
}
};
//-----------------------------------------------------------------------------------------------------------------------------------
class StarFinderButton
{
public:
inline StarFinderButton() {}
inline void init(byte p)
{
pin = p;
pinMode(pin,INPUT);
digitalWrite(pin,HIGH);
last_status_millis = millis();
last_status = HIGH;
initialing = 0;
}
inline void update(byte& s)
{
s = 0x00;
if (digitalRead(pin) == HIGH)
{
if (last_status == LOW)
{
if (!initialing) { if (millis() - last_status_millis < 1000) s = 0x01; }
else initialing = 0;
last_status = HIGH;
last_status_millis = millis();
}
}
else
{
if (last_status == HIGH)
{
if (initialing) initialing = 0;
last_status = LOW;
last_status_millis = millis();
}
else
{
if (!initialing) { if (millis() - last_status_millis >= 1500) s = 0x10; }
}
}
}
inline bool clicked(byte s) { return ( (s & 0x01) == 0x01 ); }
inline bool pushed(byte s) { return ( (s & 0x10) == 0x10 ); }
inline bool pushed() { return (digitalRead(pin) == LOW); }
inline void reinit() {initialing = 1; }
private:
byte pin;
int last_status;
unsigned long last_status_millis;
byte initialing;
};
//-----------------------------------------------------------------------------------------------------------------------------------
class N900
{
public:
inline N900() : lbuf(0) {}
inline void init() {}
// N900 -> Arduino
inline bool findStar(int* yaw, int* pitch, char* name)
{
int f = Serial.read();
if (f == -1) return false;
buf[lbuf++] = (char)f;
if (lbuf < cmd_length) return false;
lbuf = 0;
if (strncmp((char*)buf,"STAR",4) != 0)
{
Serial.flush();
return false;
}
byte c;
// 2 bytes (tot 6)
c = buf[4]; *yaw = (c<<8) & 0xFF00;
c = buf[5]; *yaw = *yaw | (c & 0x00FF);
// 2 bytes (tot 8)
c = buf[6]; *pitch = (c<<8) & 0xFF00;
c = buf[7]; *pitch = *pitch | (c & 0x00FF);
// 60 bytes (tot 68)
for (int i=0; i<60; i++) name[i] = buf[8+i];
return true;
}
// Arduino -> N900
inline void searchStarName(short yaw, short pitch, float vx, float vy, float vz)
{
Serial.flush();
Serial.print("STAR");
// yaw
Serial.write( (byte) ((yaw>>8) & 0x00FF) );
Serial.write( (byte) ((yaw ) & 0x00FF) );
// pitch
Serial.write( (byte) ((pitch>>8) & 0x00FF) );
Serial.write( (byte) ((pitch ) & 0x00FF) );
byte b3,b2,b1,b0;
// Direction X
UTIL::float2byte(vx, b3,b2,b1,b0);
Serial.write(b3);
Serial.write(b2);
Serial.write(b1);
Serial.write(b0);
// Direction Y
UTIL::float2byte(vy, b3,b2,b1,b0);
Serial.write(b3);
Serial.write(b2);
Serial.write(b1);
Serial.write(b0);
// Direction Z
UTIL::float2byte(vz, b3,b2,b1,b0);
Serial.write(b3);
Serial.write(b2);
Serial.write(b1);
Serial.write(b0);
}
// N900 -> Arduino
inline bool starNameFound(char* star_name)
{
int f = Serial.read();
if (f == -1) return false;
buf[lbuf++] = (char)f;
if (lbuf < answer_length) return false;
lbuf = 0;
for (int i=0; i<answer_length; i++) star_name[i] = buf[i];
return true;
}
inline void get_star_coords(char* star_name)
{
Serial.flush();
Serial.print("MOON");
// 16 bytes
for (int i=0; i<strlen(star_name); ++i) Serial.write( (byte)star_name[i] );
for (int i=strlen(star_name); i<16; ++i) Serial.write( (byte)0x00 );
}
inline bool star_coords_found(float& vxm, float& vym, float& vzm)
{
int f = Serial.read();
if (f == -1) return false;
buf[lbuf++] = (char)f;
if (lbuf < calibration_length) return false;
lbuf = 0;
vxm = UTIL::byte2float(buf[0], buf[1], buf[2], buf[3]);
vym = UTIL::byte2float(buf[4], buf[5], buf[6], buf[7]);
vzm = UTIL::byte2float(buf[8], buf[9], buf[10], buf[11]);
return true;
}
inline bool incoming_request()
{
return ( lbuf > 0 ? true : false );
}
private:
int lbuf;
byte buf[100];
// exactly 68 chars from N900
static const int cmd_length = 68;
// exactly 60 chars from N900
static const int answer_length = 60;
// 12 byte from N900 for calibration : 4vx 4vy 4vz Direction Vector
static const int calibration_length = 12;
};
//-----------------------------------------------------------------------------------------------------------------------------------
class StarFinder
{
public:
enum Stati
{
Compass = 1,
Wait_Lcd_to_Find_in_the_Sky = 2,
Wait_Click_to_Find_in_the_Sky = 3,
Find_in_the_Sky = 4,
Search_Star_Name = 5,
Wait_for_Star_Name = 6,
Star_Name_Found = 7,
No_Answer_from_Stellarium = 8,
Wait_Click_to_Return_Compass = 9,
Calibrate = 10,
Calibrate_Wait_Click = 11,
Calibrate_Wait_Answer = 12,
Calibrated = 13,
Calibrated_Wait_Click = 14,
Calibrate_Ask_Clik = 15,
Calibrate_Menu_Start = 16,
Calibrate_Menu = 17,
Resetted = 18,
Calibrate_Ask_Clik_2 = 19,
Calibrate_Menu_Wait_Click = 20,
Calibrate_Menu_Execute = 21,
Calibrate_Ask_Clik_Wait_Click = 22
};
inline StarFinder() : stato(Compass), pitch(0), roll(0), yaw(0) {}
inline void init(byte pin_button, byte lcd_avail, byte spi_select)
{
int i;
Serial.begin(57600);
Wire.begin();
pinMode(spi_select,OUTPUT);
SPI.begin();
SPI.setBitOrder(MSBFIRST);
SPI.setClockDivider(SPI_CLOCK_DIV8);
SPI.setDataMode(SPI_MODE0);
// HIGH = Reset Calibration
pinMode(A0,INPUT);
// HIGH = Debug Print to UART
pinMode(A1,INPUT);
spi_uart.init();
stellarium.init();
accel.init();
magn.init();
gyro.init();
gyro.zero_cal();
lcd.init(lcd_avail, &spi_uart);
button.init(pin_button);
if (digitalRead(A0) == HIGH) init_eeprom();
read_eeprom();
// If we don't delay enought, the LCD locks!
delay(3000);
last_millis_ahrs =
last_millis_lcd_update =
last_millis_choose_calibration_star =
last_millis_stellarium_query = millis();
calibration_running = 0;
}
inline int get_yaw() { return yaw; }
inline int get_pitch() { return pitch; }
inline int get_roll() { return roll; }
inline void update()
{
byte s;
int lc;
char c[10];
update_ahrs();
button.update(s);
switch (stato)
{
case Compass:
{
if (lcd.avail()) lcd.compass(yaw,pitch);
// avoid to hog the cpu
if (millis() - last_millis_stellarium_query > 100 || stellarium.incoming_request())
{
last_millis_stellarium_query = millis();
if (stellarium.findStar(&star_yaw, &star_pitch, star_name)) stato = Wait_Lcd_to_Find_in_the_Sky;
}
if (button.pushed(s)) stato = Calibrate_Menu_Start;
else if (button.clicked(s)) stato = Search_Star_Name;
break;
}
/////////////////////////////////////////////////////
case Wait_Lcd_to_Find_in_the_Sky:
{
if (lcd.avail())
{
// exactly 60 bytes msg to LCD
strncpy(lcd_msg,"Find Star: ",16);
strncpy(&lcd_msg[16],star_name,44);
lcd.message(lcd_msg);
stato = Wait_Click_to_Find_in_the_Sky;
}
break;
}
case Wait_Click_to_Find_in_the_Sky:
{
if (button.clicked(s)) stato = Find_in_the_Sky;
break;
}
case Find_in_the_Sky:
{
if (lcd.avail()) lcd.find(yaw,pitch, star_yaw,star_pitch);
if (button.clicked(s)) stato = Compass;
break;
}
/////////////////////////////////////////////////////
case Calibrate_Menu_Start:
{
if (lcd.avail())
{
// exactly 60 bytes msg to LCD
strncpy(&lcd_msg[00],"Calibrate: Click",16);
strncpy(&lcd_msg[16],"to Change Option",16);
strncpy(&lcd_msg[32],"Push to Choose. ",16);
strncpy(&lcd_msg[48]," ",12);
lcd.message(lcd_msg);
stato = Calibrate_Menu_Wait_Click;
}
break;
}
case Calibrate_Menu_Wait_Click:
{
if (button.clicked(s))
{
current_menu_option = EXIT;
stato = Calibrate_Menu;
}
break;
}
case Calibrate_Menu:
{
if (lcd.avail())
{
if (current_menu_option == EXIT) strcpy(lcd_msg,"EXIT");
else if (current_menu_option == SAVE_AND_EXIT) strcpy(lcd_msg,"SAVE AND EXIT");
else if (current_menu_option == ENABLE_DISABLE) strcpy(lcd_msg,offsets_enabled ? "DISABLE":"ENABLE");
else strcpy(lcd_msg,stars [current_menu_option - POLARIS]);
for (int i=strlen(lcd_msg); i<60; ++i) lcd_msg[i] = ' ';
lcd.message(lcd_msg);
stato = Calibrate_Menu_Execute;
}
break;
}
case Calibrate_Menu_Execute:
{
if (button.clicked(s))
{
current_menu_option++;
if (current_menu_option > CAPELLA) current_menu_option = EXIT;
stato = Calibrate_Menu;
}
else if (button.pushed(s))
{
if (current_menu_option == EXIT) stato = Compass;
else if (current_menu_option == ENABLE_DISABLE) { offsets_enabled = ! offsets_enabled; stato = Calibrate_Menu; }
else if (current_menu_option == SAVE_AND_EXIT) { save_eeprom(); stato = Compass; }
else stato = Calibrate;
button.reinit();
}
break;
}
case Calibrate:
{
stellarium.get_star_coords( stars [current_menu_option - POLARIS] );
last_millis_stellarium_calibrate = millis();
stato = Calibrate_Wait_Answer;
break;
}
case Calibrate_Wait_Answer:
{
if (millis() - last_millis_stellarium_calibrate > 3000)
{
stato = No_Answer_from_Stellarium;
break;
}
if (stellarium.star_coords_found(vx_star_real, vy_star_real, vz_star_real))
stato = Calibrate_Ask_Clik;
break;
}
case Calibrate_Ask_Clik:
{
int is = current_menu_option - POLARIS;
int l = strlen(stars[is]);
if (lcd.avail())
{
strncpy(&lcd_msg[00],"Click ",16);
strncpy(&lcd_msg[06],stars[is],l>10?10:l);
strncpy(&lcd_msg[16],"many times, and ",16);
strncpy(&lcd_msg[32],"Push when Done ",16);
strncpy(&lcd_msg[48]," ",12);
lcd.message(lcd_msg);
stato = Calibrate_Ask_Clik_Wait_Click;
}
break;
}
case Calibrate_Ask_Clik_Wait_Click:
{
if (button.clicked(s)) stato = Calibrate_Ask_Clik_2;
break;
}
case Calibrate_Ask_Clik_2:
{
if (lcd.avail())
{
strncpy(&lcd_msg[00],"Or Push Soon ",16);
strncpy(&lcd_msg[16],"to reset Star ",16);
strncpy(&lcd_msg[32],"Calibration ",16);
strncpy(&lcd_msg[48]," ",12);
lcd.message(lcd_msg);
stato = Calibrate_Wait_Click;
// Disable adding offsets during the calibration
calibration_running = 1;
star_calibration_status = 0;
star_calibration_yaw = 0;
star_calibration_pitch = 0;
}
break;
}
case Calibrate_Wait_Click:
{
int l;
char t[32];
int is = current_menu_option - POLARIS;
if (button.clicked(s))
{
// The Location of the Calibration Stars are converted
// and saved in Polar Coordinates.
// Yaw is divided by 2, to use only 1 byte
float star_yaw, star_pitch;
UTIL::vec2euler(vx_star_real, vy_star_real, vz_star_real, star_yaw, star_pitch);
stars_locations[is][0] = (byte) round(star_yaw / 2);
stars_locations[is][1] = (byte) round(star_pitch);
// (vx,vy,vz) does not contain offsets, when "calibration_running==1"
float star_yaw_click, star_pitch_click;
UTIL::vec2euler(vx,vy,vz, star_yaw_click, star_pitch_click);
float dyaw = UTIL::yaw_between(star_yaw_click, star_yaw);
float dpitch = (star_pitch - star_pitch_click);
star_calibration_yaw += dyaw;
star_calibration_pitch += dpitch;
star_calibration_status++ ;
UTIL::long2char( (long) round(dyaw), t);
l = strlen(t);
strncpy(&lcd_msg[00],"yaw: ",16);
strncpy(&lcd_msg[05],t, l>11?11:l);
UTIL::long2char( (long) round(dpitch), t);
l = strlen(t);
strncpy(&lcd_msg[16],"pitch: ",16);
strncpy(&lcd_msg[23],t, l>9?9:l);
UTIL::long2char( (long) round(star_calibration_yaw / star_calibration_status), t);
l = strlen(t);
strncpy(&lcd_msg[32],"myaw: ",16);
strncpy(&lcd_msg[38],t, l>10?10:l);
UTIL::long2char( (long) round(star_calibration_pitch / star_calibration_status), t);
l = strlen(t);
strncpy(&lcd_msg[48],"mpitch: ",12);
strncpy(&lcd_msg[56],t, l>4?4:l);
lcd.message(lcd_msg);
}
// Disable the calibration of the Single Star
// Only before the first click.
else if (button.pushed(s))
{
if (star_calibration_status == 0)
{
// bitClear
stars_offsets_enabled = ( stars_offsets_enabled & ~(0x0001<<is) );
stato = Resetted;
calibration_running = 0;
}
else
{
stars_offsets[is][0] = (byte)(char) round( star_calibration_yaw / star_calibration_status );
stars_offsets[is][1] = (byte)(char) round( star_calibration_pitch / star_calibration_status );
// bitSet
stars_offsets_enabled = ( stars_offsets_enabled | (0x0001<<is) );
stato = Calibrated;
calibration_running = 0;
}
button.reinit();
}
break;
}
case Resetted:
{
if (lcd.avail())
{
strncpy(&lcd_msg[00],"Resetted ",16);
strncpy(&lcd_msg[16]," ",16);
strncpy(&lcd_msg[16], stars[current_menu_option - POLARIS], 10);
strncpy(&lcd_msg[32]," ",16);
strncpy(&lcd_msg[48]," ",12);
lcd.message(lcd_msg);
stato = Calibrated_Wait_Click;
}
break;
}
case Calibrated:
{
if (lcd.avail())
{
int is = current_menu_option - POLARIS;
strncpy(&lcd_msg[00],"Calibrated ",16);
strncpy(&lcd_msg[16]," ",16);
strncpy(&lcd_msg[16], stars[is], 10);
UTIL::long2char( (long)(char) stars_offsets[is][0], c);
int lc = strlen(c);
strncpy(&lcd_msg[32],"yaw ",4);
strncpy(&lcd_msg[36],c,lc);
for (int i=36+lc; i<48; ++i) lcd_msg[i]=' ';
UTIL::long2char( (long)(char) stars_offsets[is][1], c);
lc = strlen(c);
strncpy(&lcd_msg[48],"pitch ",6);
strncpy(&lcd_msg[52],c,lc);
for (int i=52+lc; i<60; ++i) lcd_msg[i]=' ';
lcd.message(lcd_msg);
stato = Calibrated_Wait_Click;
}
break;
}
case Calibrated_Wait_Click:
{
if (button.clicked(s)) stato = Calibrate_Menu;
break;
}
/////////////////////////////////////////////////////
case Search_Star_Name:
{
stellarium.searchStarName(yaw,pitch, vx,vy,vz);
stato = Wait_for_Star_Name;
last_millis_stellarium_search = millis();
break;
}
case Wait_for_Star_Name:
{
if (millis() - last_millis_stellarium_search > 3000)
{
stato = No_Answer_from_Stellarium;
break;
}
if (stellarium.starNameFound(star_name)) stato = Star_Name_Found;
break;
}
case Star_Name_Found:
{
if (lcd.avail())
{
// exactly 60 bytes msg to LCD
strncpy(lcd_msg,"Found Star: ",16);
strncpy(&lcd_msg[16],star_name,44);
lcd.message(lcd_msg);
stato = Wait_Click_to_Return_Compass;
}
break;
}
case No_Answer_from_Stellarium:
{
if (lcd.avail())
{
strncpy(lcd_msg,"No Answer ",16);
strncpy(&lcd_msg[16],"from Stellarium",16);
for (int i=32; i<60; ++i) lcd_msg[i]=0;
lcd.message(lcd_msg);
stato = Wait_Click_to_Return_Compass;
}
break;
}
case Wait_Click_to_Return_Compass:
{
if (button.clicked(s)) stato = Compass;
break;
}
/////////////////////////////////////////////////////
default:
break;
}
}
private:
unsigned long last_millis_ahrs;
unsigned long last_millis_lcd_update;
unsigned long last_millis_stellarium_query;
unsigned long last_millis_stellarium_search;
unsigned long last_millis_stellarium_calibrate;
unsigned long last_millis_choose_calibration_star;
int star_yaw, star_pitch;
char star_name[60];
char lcd_msg[60];
byte stato;
byte star_calibration_status;
float star_calibration_yaw, star_calibration_pitch;
float vx, vy, vz;
float vx_star_real, vy_star_real, vz_star_real;
int pitch, roll, yaw;
int pitch_offset,yaw_offset;
byte calibration_running;
byte offsets_enabled;
int stars_offsets_enabled;
byte stars_offsets[13][2];
byte stars_locations[13][2];
byte current_menu_option;
enum Menu_Options
{
EXIT = 0,
SAVE_AND_EXIT = 1,
ENABLE_DISABLE = 2,
POLARIS = 3,
VEGA = 4,
ALKAID = 5,
MERAK = 6,
ARCTURUS = 7,
DENEB = 8,
ANTARES = 9,
SPICA = 10,
REGULUS = 11,
ALPHARD = 12,
PROCYON = 13,
POLLUS = 14,
CAPELLA = 15
};
static char* stars[13];
SpiUartDevice spi_uart;
ULCD144 lcd;
N900 stellarium;
StarFinderButton button;
ADXL345 accel;
HMC5843 magn;
ITG3200 gyro;
AHRS ahrs;
inline void read_eeprom()
{
byte c,d;
offsets_enabled = EEPROM.read(0);
c = EEPROM.read(1);
d = EEPROM.read(2);
stars_offsets_enabled =
(((unsigned int)c<<8) & 0xFF00) |
(((unsigned int)d ) & 0x00FF) ;
for (int i=0; i<13; ++i)
{
c = EEPROM.read(3 + 2*i);
stars_offsets [i][0] = c;
c = EEPROM.read(3 + 2*i + 1);
stars_offsets [i][1] = c;
c = EEPROM.read(3 + 26 + 2*i);
stars_locations [i][0] = c;
c = EEPROM.read(3 + 26 + 2*i + 1);
stars_locations [i][1] = c;
}
}
inline void init_eeprom()
{
offsets_enabled = 0;
stars_offsets_enabled = 0;
for (int i=0; i<13; ++i)
{
stars_offsets[i][0] = 0;
stars_offsets[i][1] = 0;
stars_locations[i][0] = 0;
stars_locations[i][1] = 0;
save_eeprom();
}
}
inline void save_eeprom()
{
byte c;
EEPROM.write(0,offsets_enabled);
c = (byte) ((stars_offsets_enabled>>8) & 0x00FF);
EEPROM.write(1,c);
c = (byte) (stars_offsets_enabled & 0x00FF);
EEPROM.write(2,c);
for (int i=0; i<13; ++i)
{
c = stars_offsets [i][0];
EEPROM.write(3 + 2*i, c);
c = stars_offsets [i][1];
EEPROM.write(3 + 2*i + 1, c);
c = stars_locations [i][0];
EEPROM.write(3 + 26 + 2*i, c);
c = stars_locations [i][1];
EEPROM.write(3 + 26 + 2*i + 1, c);
}
}
inline void update_ahrs()
{
unsigned long millis_elapsed = millis() - last_millis_ahrs;
last_millis_ahrs = millis();
float gd[3], ad[3], md[3];
gyro.get_xyz(gd);
accel.get_xyz(ad);
magn.get_xyz(md);
ahrs.update(gd[0],gd[1],gd[2], ad[0],ad[1],ad[2], - md[1], md[0], - md[2], millis_elapsed / 2000.0);
UTIL::quat2euler(ahrs.q0,ahrs.q1,ahrs.q2,ahrs.q3, &yaw,&pitch,&roll);
UTIL::quat2vec(ahrs.q0,ahrs.q1,ahrs.q2,ahrs.q3, &vx,&vy,&vz);
if (offsets_enabled && !calibration_running)
{
if (millis() - last_millis_choose_calibration_star > 300)
{
int k,dy,dp;
pitch_offset = yaw_offset = 0;
k = dy = dp = 999;
for (int i=0; i<13; ++i)
{
if ( ((stars_offsets_enabled>>i) & 0x0001) == 0) continue;
int sly = ((unsigned int)stars_locations[i][0]) * 2;
int slp = ((unsigned int)stars_locations[i][1]);
int dys = abs( UTIL::yaw_between(yaw,sly) );
int dps = abs(slp - pitch);
if (dys < dy && dps < dp)
{
k = i;
dy = dys;
dp = dps;
}
}
if (k < 999)
{
yaw_offset = (int)(char)stars_offsets[k][0];
pitch_offset = (int)(char)stars_offsets[k][1];
}
last_millis_choose_calibration_star = millis();
}
// re-normalize yaw in [0,359] after adding the offset
pitch += pitch_offset;
yaw += yaw_offset;
if (yaw < 0) yaw += 360;
if (yaw >= 360) yaw -= 360;
UTIL::roty(vx,vy,vz, pitch_offset);
UTIL::rotz(vx,vy,vz, - yaw_offset );
}
}
};
char* StarFinder::stars[13] =
{
"Polaris",
"Vega",
"Alkaid",
"Merak",
"Arcturus",
"Deneb",
"Antares",
"Spica",
"Regulus",
"Alphard",
"Procyon",
"Pollux",
"Capella"
};
//-----------------------------------------------------------------------------------------------------------------------------------
StarFinder finder;
void setup() { finder.init(9/*button*/, 8/*lcd_ok*/, 10/*spi_select*/); }
void loop() { finder.update(); }
The Stellarium Code
MobileGui: the original code It can also be downloaded at this link
And this is The modded code
Stellarium: the original code (not everything)
And this is the modded code
We must activate Bluetooth on N900, and click the Devices button, to connect to BlueSmirf. The first time il will do the pairing, asking for the default password of BlueSmirf: 1234
Now we must configure the serial port rfcomm0:
Add the serial service to the local sdpd if you don't have it already:
$ sdptool add SP
Serial Port service registered
To check if the service was added:
$ sdptool browse local
[...]
Service Name: Serial Port
Service Description: COM Port
Service Provider: BlueZ
[...]
Protocol Descriptor List:
"L2CAP" (0x0100)
"RFCOMM" (0x0003)
Channel: 1
[...]
Find the Bluetooth address of the BlueSMiRF:
$ hcitool scan
Scanning ...
00:06:66:03:73:B3 FireFly-73B3
The channel number of your modem should be 1, but if you want to check it out just in case:
$ sdptool records 00:06:66:03:73:B3
Service Name: SPP
[...]
Protocol Descriptor List:
"L2CAP" (0x0100)
"RFCOMM" (0x0003)
Channel: 1
Edit your rfcomm.conf:
$ vi /etc/bluetooth/rfcomm.conf
Add the lines below:
rfcomm0 {
# Automatically bind the device at startup
bind yes;
# Bluetooth address of the device
device 00:06:66:03:73:B3;
# RFCOMM channel for the connection
channel 1;
# Description of the connection
comment "Arduino";
}
With this configuration (byRicardo Ferreira) my N900 did not connected automatically to Bluesmirf (?). So a added this step:
Inside /etc/init.d/rcS add the line
rfcomm bind rfcomm0
before the line exit 0 at the end
With this configuration, when Stellarium open the /dev/rfcomm0 serial port, it connects automatically to Bluesmirf: the red led stop flashing, and the green led lights up.
This is the 2 layer PCB for the LCD, made with DesignSpark (free).
This is the Gerber file of Top Layer
This is the Gerber file of the Bottom Layer
This is the Gerber file of Top Silkscreen
This is the Gerber file of Botton Silkscreen
I have not made a Schematic, but it is easy, and the silkscreen should be enough.
This is my first PCB ever, and you can see is not very good. I used Press 'n Peel sheets, but the two layers were not perfectly aligned
...and the drill was a sh..
Now i have bought a Proxxon (like a Dremel)
The final result is decent.
I have filled the Vias with copper wire, and then soldered.
A second 5 pin's strip has been added for future update of the LCD code.
The LCD is ready for the BOX.
This is the 2 layer PCB with Arduino and the Bluetooth
This is the Gerber file of Top Layer
This is the Gerber file of the Bottom Layer
This second PCB is a bit better. The two layers are quite aligned. I have drilled some holes with a 0.5mm tip, and have aligned the second Press n Peel looking for the light through the holes.
Well, i had forgotten some pads for unused Arduino pins: i have drilled some holes later...
The Button, and tha cap stolen from a cheap led torch
Two switches, for the main power and for the Laser.
All the Stuff connected, ready for the BOX.
I have used Expanded PVC, wich can be easily worked with a cutter, and glued with Attack.
I wanted to use also Lexan for the LCD, but it is too difficult to work it: the lcd is directly exposed.
The Spacers
We must use Flex Gel Attak. The normal Attak DOES NOT do the Job.
n
The Button
The 
Switches
All the stuff glued to
gether !
That's All!
Max Pix, 6 July 2011
Some modding
31 July 2011
Well, i have solved some problems:
- The Laser works fine for some seconds, and then it became very, very dim. You can barely see it. I learned that some laser modules have an APC (Automatic Power Control) to mantain a strong beam, and bought one on a very good site in UK
- I discovered the LCD and the power circuit of the laser creates some Soft Iron Distorsion on the Sensor. So, i have repositioned the sensor in the corner of the box
- I have tried the new sensor CMPS10. It is a 6DOF (magnetometer + accelerometer), has a Processor that makes all the math, and you can get the yaw and pitch or the Raw values. It can be used with I2C or Serial Port, and is cheap: about 27 Euro. This sensor is really very good, and the Giroscope is not important for a static application. With this chip, all the AHRS fusion code is unnecessary.
-
- Now, there is also the problem of the batteries. The Laser, Arduino and the LCD drain too much current. Maybe i will try 18650 batteries, but are much bigger than AAA, and the Box will be modified.
One More Note
6 August 2011
I saw some big biases in the heading values of StarFinder, despite the offset calibration. Well, the problem was the new batteries!
I was using Brondi rechargeable 1000mh when i did the calibration, but now i have bought Duracell Supreme 1000mh, which have a bigger Hard Iron effect on the sensor. It was necessary to re-calibrate again...
Conclusion: calibrate with the batteries you will use, e don't change type of battery!
Some more Last Notes
27 February 2012
The 6DOF magnetometer was not so good (!). I returned to the Sparkfun 9DOF, using the AHRS code
I have recalibrated the Accelerometer using the MATLAB code by Sebastian O.H. Madgwick . This code works well for the magnetomer and ALSO for the Accelerometer. So, my little C program (probably buggy) calibration C program is not necessary
And much important, i have modified the Star offsets management. I think that the magnetometer calibration is not perfetc and there are bad errors (maybe we sholud collect much more points, to average the values). In the previous code i used 13 reference stars to apply some correction to the sensor values. Now, i use a matrix of offsets of 24x6 (yaw x pitch) which cut the sky in small quadrants. I have calculated this matrix with much much.... much patience, in the middle of the garden away from magnetic distorsions, using a (big) printed goniometer, and an hand-made clinometer.
Another very important note. The coordinates between N900 and StarFinder are vectorial, and NOT angular. The reason is that trigonometry (sin cos, tan, atan...) works very bad near the singularities (about 90°). The problem was that it was quite impossible find stars vey high in the sky, due to big errors. Using vectorial coordinates, the results are very good, and i can find stars quite to the zenit!