v0.32 QMC 2018-09-03 QMC5883L version for testing

Author: Eric Stockenstrom

AntTrackMavlinkv0.32_QMC/AntTrackMavlinkv0.32_QMC.ino

@@ -0,0 +1,84 @@
+void GetAzEl(const struct Location &hom, const struct Location &cur){
+/*
+ * 
+ *  Aviation Formulary V1.33, by Ed Williams
+ *  http://williams.best.vwh.net/avform.htm   
+ *
+ *  The algorithm it gives for hc_vector.az bearing between two points is 
+ *  this:
+ *
+ *   tc1=mod(atan2(sin(lon2-lon1)*cos(lat2),
+ *          cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
+ *          2*pi)
+ */
+  
+  float a, c, d, dLat, dLon;
+  
+  #if defined Debug_All || defined Debug_AzEl
+    Debug.print("hom.lat = "); Debug.print(hom.lat,7);
+    Debug.print(" hom.lon = "); Debug.print(hom.lon,7);
+    Debug.print(" hom.alt = "); Debug.print(hom.alt,0);
+    Debug.print(" cur.lat = "); Debug.print(cur.lat,7); 
+    Debug.print(" cur.lon = "); Debug.print(cur.lon,7); 
+    Debug.print(" cur.alt = "); Debug.println(cur.alt,0); 
+ #endif
+  
+  float lon1 = hom.lon / 180 * PI;  // Degrees to radians
+  float lat1 = hom.lat / 180 * PI;
+  float lon2 = cur.lon / 180 * PI;
+  float lat2 = cur.lat / 180 * PI;
+
+  //Calculate azimuth
+  a=atan2(sin(lon2-lon1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1));
+  hc_vector.az=a*180/PI;
+  if (hc_vector.az<0) hc_vector.az=360+hc_vector.az;
+ 
+  // Calculate the distance from home to craft
+  dLat = (lat2-lat1);
+  dLon = (lon2-lon1);
+  a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2); 
+  c = 2* asin(sqrt(a));  
+  d = 6371000 * c;    // Radius of the Earth is 6371km
+  hc_vector.dist = d;
+
+  // Calculate elevation
+  int16_t altR = cur.alt - hom.alt;  // Relative alt
+
+  altR = altR * LimitCloseToHomeError(d, altR);
+  
+  hc_vector.el=atan(altR/d);
+  hc_vector.el=hc_vector.el*360/(2*PI);     // Radians to degrees
+  
+  #if defined Debug_All || defined Debug_AzEl
+  if (hc_vector.dist >= minDist) {  // Otherwise calculations are unreliable
+    float elapsed = millis() - startup_millis;
+    elapsed /=1000;
+    Debug.print("  hc_vector.az= "); Debug.print(hc_vector.az); 
+    Debug.print("  hc_vector.el= "); Debug.print(hc_vector.el);
+    Debug.print("  hc_vector.dist= "); Debug.print(hc_vector.dist);
+    Debug.print("  Elapsed= "); Debug.println(elapsed, 3);
+  }
+  else {
+     Debug.print(" hc_vector.dist = "); Debug.print(hc_vector.dist); 
+     Debug.print("  < minDist = "); Debug.println(minDist);
+  }
+  #endif
+  
+  return;
+}
+//********************************************************
+//  Limit close-to-home elevation error due to poor vertical GPS accuracy 
+float LimitCloseToHomeError(float dist, int16_t alt) {  
+  
+float h_norm = 10.0;     // Normal at 10m dist
+float v_norm = 5.0;      // Normal at 5m alt
+float h_ratio, v_ratio, t_ratio;  
+
+  h_ratio = pow((dist / h_norm), 2);  
+  v_ratio = pow((float)(alt / v_norm), 2); 
+  t_ratio = h_ratio * v_ratio;
+  if (t_ratio > 1.0) 
+    t_ratio = 1;
+  return t_ratio;  
+}
+

AntTrackMavlinkv0.32_QMC/AntTrackMavlinkv0.32_QMC.ino.generic_stm32f103c.bin

@@ -0,0 +1,61 @@
+#if (Heading_Source == 3)  // Tracker_Compass   //  else don't compile this module
+#include <Wire.h>
+#include <QMC5883L.h>
+
+QMC5883L compass;
+
+#ifndef Debug
+  #define Debug               Serial         // USB 
+#endif
+
+// Get the mag declination of your location here http://www.magnetic-declination.com/ and insert in # defined in next line
+#define DECLINATION  -18.9 // In degrees
+ 
+
+//******************************************************************************
+uint8_t Initialise_Compass() {
+
+  // There is no check for missing magnetometer in this library. Possibly scan for known address
+  
+  Wire.begin();
+  Serial.begin(9600);
+  compass.init();
+
+ return 1;
+}
+
+//******************************************************************************
+float GetMagHeading() {
+  int x,y,z;
+  compass.read(&x,&y,&z);
+  
+  float fHeading = RadToDeg(atan2(y, x));  //All in degrees now
+  
+  fHeading += DECLINATION;  // Add magnetic declination
+
+  fHeading = Normalise_360(fHeading);
+  
+  #if defined Debug_All || defined Debug_Compass
+    /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
+ //   Debug.print("X: "); Debug.print(x); Debug.print("  ");
+ //   Debug.print("Y: "); Debug.print(y); Debug.print("  ");
+ //   Debug.print("Z: "); Debug.print(z); Debug.println("  ");
+    Debug.print("Heading = "); Debug.println(fHeading,0); 
+  #endif   
+
+  return fHeading;
+}
+
+//******************************************************************************
+
+int16_t Normalise_360(int16_t arg) {
+  if (arg < 0) arg += 360;
+  if (arg > 359) arg -= 360;
+  return arg;
+}
+//***************************************************
+float RadToDeg (float _Rad) {
+  return _Rad * 180 / PI;  
+}
+//***************************************************
+#endif

AntTrackMavlinkv0.32_QMC/AzEl.ino

@@ -0,0 +1,107 @@
+
+
+void  PositionServos(float Az, float El, float hmHdg) {
+
+  // Note my 180 degree servos have a PWM range of 700 through 2300 microseconds 
+  // Your's may differ 
+  // Uncomment TestServos() in setup() code to observe how well your servos reach their limits
+
+#ifdef Az_Servo_360           // Set the limits of the servos here
+  int16_t llAz = 0;           // Az lower limit in degrees
+  int16_t ulAz = 360;         // Az upper limit in degrees
+  int16_t llEl = 0;           // El lower limit in degrees
+  int16_t ulEl = 90;          // El upper limit in degrees
+  int16_t MinAzPWM = 650;     // Self explanatory
+  int16_t MaxAzPWM = 2400;
+  int16_t MinElPWM = 1125;
+  int16_t MaxElPWM = 1875;
+#else
+  int16_t llAz = 0;     
+  int16_t ulAz = 180;    
+  int16_t llEl = 0;
+  int16_t ulEl = 180;
+  int16_t MaxAzPWM = 2300;
+  int16_t MinAzPWM = 700;
+  int16_t MaxElPWM = 2300;
+  int16_t MinElPWM = 700;
+#endif
+  #if defined Debug_All || defined Debug_Servos
+  Debug.print("Az = " );
+  Debug.print(Az);
+  Debug.print("\t El = ");
+  Debug.print(El);
+  Debug.print("\t hmHdg = " );
+  Debug.println(hmHdg);
+#endif
+  int16_t pntAz = pointAz(Az, hmHdg);  // Remap Azimuth into a 180 degree window with home-heading centre, negative behind us
+
+  #ifndef Az_Servo_360            // Note if NOT. Do this for 180, not for 360 azimuth servo
+  if (pntAz<0) {        // Pointing direction is negative, so it needs to point behind us,
+    pntAz = 0 - pntAz;    // so flip the frame of reference over and mirror it (neg to pos)
+    El = 180 - El;        // and make the el servo reach over and behind
+  }
+  #endif 
+   
+  // If the craft moves out of this restricted field of reference, servos should wait at the most recent in-field position
+  // This code is not neccessary with two 180 degree servos covering an entire hemisphere
+
+  if ((pntAz>=llAz) && (pntAz<=ulAz) && (El>=llEl) && (El<=ulEl)) {   // Our restricted FOV
+    LastGoodpntAz = pntAz;
+    LastGoodEl = El;
+  }
+
+  azPWM = map(LastGoodpntAz, ulAz, llAz, MinAzPWM, MaxAzPWM);  // Map the az / el to the servo PWMs
+  elPWM = map(LastGoodEl, ulEl, llEl, MinElPWM, MaxElPWM);     // Servos happen to be mounted such that action is reversed
+  
+  azServo.writeMicroseconds(azPWM);
+  elServo.writeMicroseconds(elPWM);
+  
+  #if defined Debug_All || defined Debug_Servos
+  Debug.print("pntAz = " );
+  Debug.print(pntAz);
+  Debug.print(" El = ");
+  Debug.print(El);
+  Debug.print(" LastGoodpntAz = " );
+  Debug.print(LastGoodpntAz);
+  Debug.print(" LastGoodEl = ");
+  Debug.print(LastGoodEl);
+  Debug.print(" hmHdg = " );
+  Debug.print(hmHdg);
+  Debug.print(" azPWM = " );
+  Debug.print(azPWM);
+  Debug.print(" elPWM = ");
+  Debug.println(elPWM);
+  #endif
+  
+}
+ //*************************************************** 
+  int16_t pointAz(int16_t Azin, int16_t rhmHdg)  {
+  // Remap the craft's absolute Az from the home position to the 180 degree azimuth aperture facing us, and behind us
+  // 0 degrees on the left and 180 on the right, same but mirrored and negative behind us 180 on the right 0 on the left.
+  // Home (antenna) heading is at the centre (90 deg) of the 180 degree forward azimuth aperture
+  // pointAz is the heading to the craft from the home position, and thus where to point the antenna, 
+  //  RELATIVE to our frame of reference.
+  
+  int16_t pntAz= Azin;
+
+  #if defined Debug_All || defined Debug_Servos
+//  Debug.print("pointAz = ");
+//  Debug.print(pntAz);
+//  Debug.print(" rhmHdg = ");
+//  Debug.println(rhmHdg);
+  #endif
+
+  pntAz = pntAz - rhmHdg + 90;    // Make heading relative to 90 degrees in front of us
+  if (pntAz<0) pntAz = 90-pntAz;  // Correct the 90 deg boundary if necessary
+  
+  #ifndef Az_Servo_360            // Note if not. Do this for 180, not for 360 azimuth servo
+    if ((pntAz>180) && (pntAz<=360)) pntAz=180-pntAz;  // Mirror the hemisphere behind us, and make it negative
+  #endif  
+    
+  if (pntAz>360) pntAz=pntAz-360;   // All the way round to positive territory again
+
+  return pntAz;
+}
+//***************************************************
+
+

AntTrackMavlinkv0.32_QMC/Compass.ino

@@ -0,0 +1,56 @@
+/*
+  QMC5883L.cpp - QMC5883L library
+  Copyright (c) 2017 e-Gizmo Mechatronix Central
+  Rewritten by Amoree.  All right reserved.
+  July 10,2017
+
+  SET continuous measurement mode
+  OSR = 512
+  Full Scale Range = 8G(Gauss)
+  ODR = 200HZ
+ 
+*/
+
+#include "QMC5883L.h"
+
+#include <Wire.h>
+
+void QMC5883L::setAddress(uint8_t addr){
+  address = addr;
+}
+
+void QMC5883L::WriteReg(byte Reg,byte val){
+  Wire.beginTransmission(address); //Start
+  Wire.write(Reg); // To tell the QMC5883L to get measures continously
+  Wire.write(val); //Set up the Register
+  Wire.endTransmission();
+}
+
+void QMC5883L::init(){
+  WriteReg(0x0B,0x01);
+  //Define Set/Reset period
+  setMode(Mode_Continuous,ODR_200Hz,RNG_8G,OSR_512);
+}
+
+void QMC5883L::setMode(uint16_t mode,uint16_t odr,uint16_t rng,uint16_t osr){
+  WriteReg(0x09,mode|odr|rng|osr);
+  Serial.println(mode|odr|rng|osr,HEX);
+}
+
+
+void QMC5883L::softReset(){
+  WriteReg(0x0A,0x80);
+}
+
+void QMC5883L::read(int* x,int* y,int* z){
+  Wire.beginTransmission(address);
+  Wire.write(0x00);
+  Wire.endTransmission();
+  Wire.requestFrom(address, 6);
+  *x = Wire.read(); //LSB  x
+  *x |= Wire.read() << 8; //MSB  x
+  *y = Wire.read(); //LSB  z
+  *y |= Wire.read() << 8; //MSB z
+  *z = Wire.read(); //LSB y
+  *z |= Wire.read() << 8; //MSB y
+}

AntTrackMavlinkv0.32_QMC/Servos.ino

@@ -0,0 +1,58 @@
+/*
+  QMC5883L.h - QMC5883L library
+  Copyright (c) 2017 e-Gizmo Mechatronix Central
+  Rewritten by Amoree.  All right reserved.
+  July 10,2017
+*/
+
+#ifndef QMC5883L_h
+#define QMC5883L_h
+
+
+#if ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#include "Wire.h"
+
+#define QMC5883L_ADDR 0x0D//The default I2C address is 0D: 0001101
+
+
+//Registers Control //0x09
+
+#define Mode_Standby    0b00000000
+#define Mode_Continuous 0b00000001
+
+#define ODR_10Hz        0b00000000
+#define ODR_50Hz        0b00000100
+#define ODR_100Hz       0b00001000
+#define ODR_200Hz       0b00001100
+
+#define RNG_2G          0b00000000
+#define RNG_8G          0b00010000
+
+#define OSR_512         0b00000000
+#define OSR_256         0b01000000
+#define OSR_128         0b10000000
+#define OSR_64          0b11000000
+
+
+class QMC5883L{
+
+public:
+void setAddress(uint8_t addr);
+void init();
+void setMode(uint16_t mode,uint16_t odr,uint16_t rng,uint16_t osr);
+void softReset();
+void read(uint16_t* x,uint16_t* y,uint16_t* z);
+void read(int* x,int* y,int* z);
+
+private:
+void WriteReg(uint8_t Reg,uint8_t val);
+uint8_t address = QMC5883L_ADDR;
+
+};
+
+#endif