View Single Post
Old Jan 29, 2013, 08:32 PM
danstrider is online now
Find More Posts by danstrider
Who needs a pilot??
danstrider's Avatar
Alexandria, VA
Joined Jul 2002
1,222 Posts
In Rolf's spirit of this open-source thread, at the end of this post is the code I meant to fly tonight. In return, you have to post code updates and changes, feed the internet!!

Speaking of flying, I tried out the total energy probe this evening at the local soccer field. While it's still much under-compensated (I can hear when I push over or pull up), it sounds much much better than before. I experimented with bending the probe forward more or less. Bending less (a straighter tube) was better. Some remaining tests include moving the hole closer to the tip, using a smaller hole, using a larger diameter tube, and otherwise just continuing to play with it. Aluminum tube is cheap and this is definitely progress.

Also posted below is another terrible video with reasonable audio. You can watch the push/pull and hear the vario signal this motion. But compared to last time, not near as bad.

I haven't made any decisions about what to do with these boards because I'm still not happy enough with it. There is still some work to do, especially on ground hardware... I don't have a custom ground board with a shiny volume knob!

Dan


Video (wow yeah, compare to last flight)
Compensated DIY Arduino Vario (1 min 4 sec)


Airborne software:
Code:
// DIY Arduino Variometer, FrSky style
// All code by Rolf R Bakke, Oct 2012
//   http://www.rcgroups.com/forums/showthread.php?t=1749208
// Modifications by Dan Edwards, Nov/Dec 2012
//   Added #defines for the command names for clarity.
//   Tone is no longer generated airborne, instead data is
//     transmitted to the ground via an FrSky telemetry module.
//     A ground station parses the data and makes the tone.
//   Added the 2nd order temperature calibration routine.
//   Added a CRC check appropriate for short messages.
//   Referenced loop timer to current time, quashing a bug.

//_____ INCLUDES
#include <Wire.h>           // for altimeter sensor
#include <SoftwareSerial.h> // for inverted serial

//_____ MACROS
#define LED_PIN 13 // pin for LED

#define ADDR            0x77 // address of MS-5611
#define CMD_RESET       0x1E // ADC reset command
#define CMD_ADC_READ    0x00 // ADC read command
#define CMD_PROM_RD     0xA0 // Prom read command
#define CMD_D1_ADC_256  0x40 // D1 ADC OSR=256
#define CMD_D1_ADC_512  0x42 // D1 ADC OSR=512
#define CMD_D1_ADC_1024 0x44 // D1 ADC OSR=1024
#define CMD_D1_ADC_2048 0x46 // D1 ADC OSR=2056
#define CMD_D1_ADC_4096 0x48 // D1 ADC OSR=4096
#define CMD_D2_ADC_256  0x50 // D2 ADC OSR=256
#define CMD_D2_ADC_512  0x52 // D2 ADC OSR=512
#define CMD_D2_ADC_1024 0x54 // D2 ADC OSR=1024
#define CMD_D2_ADC_2048 0x56 // D2 ADC OSR=2056
#define CMD_D2_ADC_4096 0x58 // D2 ADC OSR=4096

//_____ DEFINITIONS
unsigned int calibData[7];

// 8-bit CRC 0x97. Table computed for from python script found at:
// http://hacromatic.com/blog/2012/08/make-your-serial-protocol-robust-with-8-bit-crc-codes/
static PROGMEM prog_uint8_t crc_table[256] = {
  0x00, 0x2f, 0x5e, 0x71, 0xbc, 0x93, 0xe2, 0xcd, 0x57, 0x78, 0x09, 0x26, 0xeb, 0xc4, 0xb5, 0x9a,
  0xae, 0x81, 0xf0, 0xdf, 0x12, 0x3d, 0x4c, 0x63, 0xf9, 0xd6, 0xa7, 0x88, 0x45, 0x6a, 0x1b, 0x34,
  0x73, 0x5c, 0x2d, 0x02, 0xcf, 0xe0, 0x91, 0xbe, 0x24, 0x0b, 0x7a, 0x55, 0x98, 0xb7, 0xc6, 0xe9,
  0xdd, 0xf2, 0x83, 0xac, 0x61, 0x4e, 0x3f, 0x10, 0x8a, 0xa5, 0xd4, 0xfb, 0x36, 0x19, 0x68, 0x47,
  0xe6, 0xc9, 0xb8, 0x97, 0x5a, 0x75, 0x04, 0x2b, 0xb1, 0x9e, 0xef, 0xc0, 0x0d, 0x22, 0x53, 0x7c,
  0x48, 0x67, 0x16, 0x39, 0xf4, 0xdb, 0xaa, 0x85, 0x1f, 0x30, 0x41, 0x6e, 0xa3, 0x8c, 0xfd, 0xd2,
  0x95, 0xba, 0xcb, 0xe4, 0x29, 0x06, 0x77, 0x58, 0xc2, 0xed, 0x9c, 0xb3, 0x7e, 0x51, 0x20, 0x0f,
  0x3b, 0x14, 0x65, 0x4a, 0x87, 0xa8, 0xd9, 0xf6, 0x6c, 0x43, 0x32, 0x1d, 0xd0, 0xff, 0x8e, 0xa1,
  0xe3, 0xcc, 0xbd, 0x92, 0x5f, 0x70, 0x01, 0x2e, 0xb4, 0x9b, 0xea, 0xc5, 0x08, 0x27, 0x56, 0x79,
  0x4d, 0x62, 0x13, 0x3c, 0xf1, 0xde, 0xaf, 0x80, 0x1a, 0x35, 0x44, 0x6b, 0xa6, 0x89, 0xf8, 0xd7,
  0x90, 0xbf, 0xce, 0xe1, 0x2c, 0x03, 0x72, 0x5d, 0xc7, 0xe8, 0x99, 0xb6, 0x7b, 0x54, 0x25, 0x0a,
  0x3e, 0x11, 0x60, 0x4f, 0x82, 0xad, 0xdc, 0xf3, 0x69, 0x46, 0x37, 0x18, 0xd5, 0xfa, 0x8b, 0xa4,
  0x05, 0x2a, 0x5b, 0x74, 0xb9, 0x96, 0xe7, 0xc8, 0x52, 0x7d, 0x0c, 0x23, 0xee, 0xc1, 0xb0, 0x9f,
  0xab, 0x84, 0xf5, 0xda, 0x17, 0x38, 0x49, 0x66, 0xfc, 0xd3, 0xa2, 0x8d, 0x40, 0x6f, 0x1e, 0x31,
  0x76, 0x59, 0x28, 0x07, 0xca, 0xe5, 0x94, 0xbb, 0x21, 0x0e, 0x7f, 0x50, 0x9d, 0xb2, 0xc3, 0xec,
  0xd8, 0xf7, 0x86, 0xa9, 0x64, 0x4b, 0x3a, 0x15, 0x8f, 0xa0, 0xd1, 0xfe, 0x33, 0x1c, 0x6d, 0x42
};

// ProMini pin 6 to D8R pin Rx, that's the important one
SoftwareSerial SerialTelem(5,6,true); // rx,tx,inverse


//********************************************************
//!  initializations and setup
//!
//!  returns nothing
//********************************************************
void setup() {
  // for user to monitor
  Serial.begin(9600);
  Serial.println("FrSky vario airborne unit");
  Serial.println("Dan the man rocks your thermal\n");
  delay(2000);

  // setup led pin
  pinMode(LED_PIN, OUTPUT);

  // setup for FrSky telemetry data transfer
  SerialTelem.begin(9600); // FrSky ONLY speaks 9600 baud

  // setup altimeter sensor
  Wire.begin();
  setupSensor();
}

//********************************************************
//!  main program loop
//!
//!  returns nothing
//********************************************************
void loop() {
  static unsigned long timer=0;
  unsigned long time;
  long pressure;
  unsigned int encodedVarioTone;

  // set loop timing
  time = millis();
  if (time >= timer) {
    toggleLED();
    
    // reference timer to millis() to get desired loop timing
    timer = time + 36; //ms

    // get pressure signal (oversampling is done onboard sensor)
    pressure = getPressure();

    // compute the vario tone and encode it for transmission.
    encodedVarioTone = computeVario(pressure);

    // send encoded user bytes
    sendUserBytes(encodedVarioTone);
  }
}

//********************************************************
//!  computes vario signal
//!
//!  returns int of encoded tone freq to send
//********************************************************
unsigned int computeVario(long pressure) {
  static boolean firstTime=true;
  static float toneFreq, toneFreqLowpass, lowpassFast=0.0, lowpassSlow=0.0;
  static int ddsAcc;
  unsigned int encodedToneFreq;

  // initialize filters
  if (firstTime) {
    lowpassFast = pressure;
    lowpassSlow = pressure;
    firstTime = false;
  }

  // compute vario signal
  lowpassFast = lowpassFast + (pressure - lowpassFast) * 0.1;
  lowpassSlow = lowpassSlow + (pressure - lowpassSlow) * 0.05;
  toneFreq = (lowpassSlow - lowpassFast) * 50;
  toneFreqLowpass = toneFreqLowpass + (toneFreq - toneFreqLowpass) * 0.1;
  toneFreq = constrain(toneFreqLowpass, -500, 500);

  // direct digital synthesizer accumulator
  // int wrapping around is the trick here
  // loop timing definitely matters when picking these constants
  ddsAcc += (toneFreq * 100) + 2000;

  // create intermittent beeping tone for ascending
  // raise ToneFreq cutoff if it's beeping while sitting on the table
  if ((ddsAcc > 0) && (toneFreq > 30)) {
    encodedToneFreq = 0xFFFF; // represents a no-tone
    //on decoding end, use: noTone(TONE_PIN);
  }
  else {
    encodedToneFreq = (unsigned int)((toneFreq+510) * 65); // max use of bytes
    //on decoding end, use: tone(TONE_PIN, encodedToneFreq/65);
  }

  return encodedToneFreq;
}

//********************************************************
//!  transmits user bytes
//!  make sure this follows 1200byte/sec FrSky max
//!
//!  returns nothing
//********************************************************
void sendUserBytes(unsigned int encodedToneFreq) {
  byte buf[3];

  // build a data word to run CRC on
  buf[0] = highByte(encodedToneFreq); // MSB
  buf[1] = lowByte(encodedToneFreq);  // LSB

  // compute CRC and add to message buffer
  buf[2] = crc_update(0,buf,2);

  // send everything at once
  SerialTelem.write(buf,3);
}

//********************************************************
//!  read pressure from MS-5611 sensor
//!
//!  returns pressure 10 to 1200mbar in 0.01mbar increments
//********************************************************
long getPressure() {
  long D1, D2, dT, P, TEMP;
  int64_t OFF, SENS;

  // read digital pressure and temperature data, MS-5611 datasheet Fig 2
  D1 = getData(CMD_D1_ADC_4096); // resolution 0.012 mbar
  D2 = getData(CMD_D2_ADC_256);  // resolution 0.012 C

  // pressure and temperature calculation, MS-5611 datasheet Fig 2
  dT   = D2 - ((long)calibData[5] << 8); // -16776960 to 16777216
  TEMP = 2000 + (((int64_t)dT * (int64_t)calibData[6]) >> 23);  // -4000 to 8500 => -40C to 85C
  OFF  = ((unsigned long)calibData[2] << 16) + (((int64_t)calibData[4] * dT) >> 7);
  SENS = ((unsigned long)calibData[1] << 15) + (((int64_t)calibData[3] * dT) >> 8);

  // 2nd order temp compensation, MS-5611 datasheet Fig 3
  long T2=0.0, tmp;
  int64_t OFF2=0, SENS2=0;
  if (TEMP < 2000) {  // lower than 20C
    T2 = ((int64_t)dT * (int64_t)dT) >> 31;
    tmp = 5*(TEMP-2000)*(TEMP-2000);
    OFF2 = tmp >> 1;
    SENS2 = tmp >> 2;
  }
  if (TEMP < -1500) {  // lower than -15C
    tmp = (TEMP+1500)*(TEMP+1500);
    OFF2 = OFF2 + 7*tmp;
    SENS2 = SENS2 + (11*tmp) >> 1;
  }
  TEMP = TEMP - T2;
  OFF  = OFF  - OFF2;
  SENS = SENS - SENS2;

  // calculate pressure
  P = (((D1 * SENS) >> 21) - OFF) >> 15;

  //Serial.println(P/100); // 10mbar to 1200mbar
  //Serial.println(TEMP/100); // -40C to 85C

  return P;
}

//********************************************************
//!  sends data requests to the sensor
//!
//!  returns raw data
//********************************************************
long getData(byte command) {
  long result = 0;
  twiSendCommand(ADDR, command);
  switch (command) {
  case CMD_D1_ADC_256:
  case CMD_D2_ADC_256: 
    delayMicroseconds(900); 
    break;
  case CMD_D1_ADC_512:
  case CMD_D2_ADC_512: 
    delay(3); 
    break;
  case CMD_D1_ADC_1024:
  case CMD_D2_ADC_1024: 
    delay(4); 
    break;
  case CMD_D1_ADC_2048:
  case CMD_D2_ADC_2048: 
    delay(6); 
    break;
  case CMD_D1_ADC_4096:
  case CMD_D2_ADC_4096: 
    delay(10); 
    break;
  }
  twiSendCommand(ADDR, CMD_ADC_READ); // command ADC to read
  Wire.requestFrom(ADDR, 3); // ADC read should return 3 bytes, MSB first
  if(Wire.available()!=3) Serial.println("Error: raw data not available");
  for (int i=0; i<=2; i++) {
    result = (result<<8) | Wire.read(); 
  }
  return result;
}

//********************************************************
//!  reads calibration parameters from sensor
//!
//!  returns nothing
//********************************************************
void setupSensor() {
  commandReset();

  for (byte i = 1; i <=6; i++) // get calibration coefficients 1-6
  {
    unsigned int low, high;

    twiSendCommand(ADDR, 0xa0 + i * 2);
    Wire.requestFrom(ADDR, 2);
    if(Wire.available()!=2) Serial.println("Error: calibration data not available");
    high = Wire.read();
    low = Wire.read();
    calibData[i] = high<<8 | low;

    Serial.print("calibration data #");
    Serial.print(i);
    Serial.print(" = ");
    Serial.println( calibData[i] ); 
  }
}

//********************************************************
//!  resets sensor and waits for it to restart
//!
//!  returns nothing
//********************************************************
void commandReset() {
  twiSendCommand(ADDR, CMD_RESET); // reset sensor
  delay(3);  // wait for the reset sequence timing
}

//********************************************************
//!  two-wire interface sends a command to an I2C address
//!
//!  returns nothing
//********************************************************
void twiSendCommand(byte address, byte command) {
  Wire.beginTransmission(address);
  if (!Wire.write(command)) Serial.println("Error: write()");
  if (Wire.endTransmission()) 
  {
    Serial.print("Error when sending command: ");
    Serial.println(command, HEX);
  }
}

//********************************************************
//!  turns LED on and off
//!
//!  returns nothing
//********************************************************
void ledOn() {  
  digitalWrite(LED_PIN,1);  
}
void ledOff() {  
  digitalWrite(LED_PIN,0);  
}
void toggleLED() {
  static boolean ledStatus = true;
  if (ledStatus) {
    ledOff();
    ledStatus = false;
  }
  else {
    ledOn();
    ledStatus = true;
  }
}    

//********************************************************
//!  8-bit CRC 0x97 appropriate for short messages with HD-4
//!  http://hacromatic.com/blog/2012/08/make-your-serial-protocol-robust-with-8-bit-crc-codes/
//!
//!  returns resultant crc byte
//********************************************************
byte crc_update(byte crc, const unsigned char *data, size_t data_len) {
  unsigned int tbl_idx;

  while (data_len--) {
    tbl_idx = ((crc >> 0) ^ *data) & 0xFF;
    crc = (pgm_read_byte(crc_table+tbl_idx) ^ (crc << 8)) & 0xFF;
    data++;
  }
  return (crc & 0xFF);
}
Ground software:
Code:
// DIY Arduino Variometer, FrSky style
// All code by Dan Edwards, Nov12-Jan13 except where noted.
// Uses hardware inverter for serial
// ProMini pin RX1 to DJT pin Txd

//_____ MACROS
#define TONE_PIN 9
#define LED_PIN 13

// FrSky constants
#define FRAMESIZE 11
#define SYNC  0x7E
#define USER  0xFD
#define STUFF 0x7D

//_____ DEFINITIONS
unsigned int encodedToneFreq=0;

// 8-bit CRC 0x97. Table computed for from python script found at:
// http://hacromatic.com/blog/2012/08/make-your-serial-protocol-robust-with-8-bit-crc-codes/
static PROGMEM prog_uint8_t crc_table[256] = {
  0x00, 0x2f, 0x5e, 0x71, 0xbc, 0x93, 0xe2, 0xcd, 0x57, 0x78, 0x09, 0x26, 0xeb, 0xc4, 0xb5, 0x9a,
  0xae, 0x81, 0xf0, 0xdf, 0x12, 0x3d, 0x4c, 0x63, 0xf9, 0xd6, 0xa7, 0x88, 0x45, 0x6a, 0x1b, 0x34,
  0x73, 0x5c, 0x2d, 0x02, 0xcf, 0xe0, 0x91, 0xbe, 0x24, 0x0b, 0x7a, 0x55, 0x98, 0xb7, 0xc6, 0xe9,
  0xdd, 0xf2, 0x83, 0xac, 0x61, 0x4e, 0x3f, 0x10, 0x8a, 0xa5, 0xd4, 0xfb, 0x36, 0x19, 0x68, 0x47,
  0xe6, 0xc9, 0xb8, 0x97, 0x5a, 0x75, 0x04, 0x2b, 0xb1, 0x9e, 0xef, 0xc0, 0x0d, 0x22, 0x53, 0x7c,
  0x48, 0x67, 0x16, 0x39, 0xf4, 0xdb, 0xaa, 0x85, 0x1f, 0x30, 0x41, 0x6e, 0xa3, 0x8c, 0xfd, 0xd2,
  0x95, 0xba, 0xcb, 0xe4, 0x29, 0x06, 0x77, 0x58, 0xc2, 0xed, 0x9c, 0xb3, 0x7e, 0x51, 0x20, 0x0f,
  0x3b, 0x14, 0x65, 0x4a, 0x87, 0xa8, 0xd9, 0xf6, 0x6c, 0x43, 0x32, 0x1d, 0xd0, 0xff, 0x8e, 0xa1,
  0xe3, 0xcc, 0xbd, 0x92, 0x5f, 0x70, 0x01, 0x2e, 0xb4, 0x9b, 0xea, 0xc5, 0x08, 0x27, 0x56, 0x79,
  0x4d, 0x62, 0x13, 0x3c, 0xf1, 0xde, 0xaf, 0x80, 0x1a, 0x35, 0x44, 0x6b, 0xa6, 0x89, 0xf8, 0xd7,
  0x90, 0xbf, 0xce, 0xe1, 0x2c, 0x03, 0x72, 0x5d, 0xc7, 0xe8, 0x99, 0xb6, 0x7b, 0x54, 0x25, 0x0a,
  0x3e, 0x11, 0x60, 0x4f, 0x82, 0xad, 0xdc, 0xf3, 0x69, 0x46, 0x37, 0x18, 0xd5, 0xfa, 0x8b, 0xa4,
  0x05, 0x2a, 0x5b, 0x74, 0xb9, 0x96, 0xe7, 0xc8, 0x52, 0x7d, 0x0c, 0x23, 0xee, 0xc1, 0xb0, 0x9f,
  0xab, 0x84, 0xf5, 0xda, 0x17, 0x38, 0x49, 0x66, 0xfc, 0xd3, 0xa2, 0x8d, 0x40, 0x6f, 0x1e, 0x31,
  0x76, 0x59, 0x28, 0x07, 0xca, 0xe5, 0x94, 0xbb, 0x21, 0x0e, 0x7f, 0x50, 0x9d, 0xb2, 0xc3, 0xec,
  0xd8, 0xf7, 0x86, 0xa9, 0x64, 0x4b, 0x3a, 0x15, 0x8f, 0xa0, 0xd1, 0xfe, 0x33, 0x1c, 0x6d, 0x42
};


//********************************************************
//!  initializations and setup
//!
//!  returns nothing
//********************************************************
void setup() {
  // initialize serial port
  Serial.begin(9600); // FrSky ONLY speaks 9600 baud
  Serial.println("FrSky vario ground unit");
  Serial.println("Dan the man rocks your thermal\n");
  delay(2000);

  // setup led pin
  pinMode(LED_PIN, OUTPUT);
}

//********************************************************
//!  main program loop
//!
//!  returns nothing
//********************************************************
void loop() {
  unsigned int varioTone=0;

  // do stuff once we find good user data
  if(getUserData()) {
    toggleLED();

    // decode encoded vario to make a tone freq
    varioTone = encodedToneFreq/65;

    // make beepy noises
    if (encodedToneFreq == 0xFFFF)
      noTone(TONE_PIN);
    else
      tone(TONE_PIN,varioTone);
  }
}

//********************************************************
//!  serial communications state machine, derived from:
//!  http://arduino.cc/forum/index.php?topic=30440.15
//!  vars made static to work if we come back to this function
//!  
//!  returns true if found a valid message
//********************************************************
boolean getUserData() {
  static int counter=0;
  static boolean stuffFlag=false;
  static byte data[FRAMESIZE];

  // spool thru serial buffer
  while (Serial.available()) {
    byte b = Serial.read();
    //Serial.println(b,HEX);

    if ((b==SYNC) && (counter==FRAMESIZE-1) && (data[0]==SYNC) && (data[1]==USER)) {
      // looks like the end of a valid frame ... so let's parse!  
      if (parseUserData(data)) return true;
    } 
    else if (b==STUFF) { // byte stuffing!
      stuffFlag = true;
      continue; // this byte only used as a flag, don't save
    } 
    else if (b==SYNC) { // start of a new frame. hopefully.
      counter = 0;
      //Serial.println("start");
    }

    // handle byte stuffing
    if (stuffFlag) {
      b ^= 0x20;
      stuffFlag = false;
    }

    // save current byte in the data array
    data[counter++] = b;
  }
  return false; // not successful, boo
}

//********************************************************
//!  this function parses the encodedToneFreq message
//!  
//!  returns true if no errors
//********************************************************
boolean parseUserData(byte *data) {
  //showUserData(data);

  // error check size
  if (data[2] != 3) { // what is FrSky doing with my data???
    //Serial.println("size err");
    return false;
  }

  // check crc first level
  if (data[6] != crc_update(0,data+4,2)) { // start at 4th position
    //Serial.println("crc err");
    return false;
  }

  // reassemble encodedToneFreq
  encodedToneFreq = data[4]*256 + data[5];
  return true;
}

//********************************************************
//!  this function just shows the user data array
//!  
//!  returns nothing
//********************************************************
void showUserData(int *data) {
  Serial.print("user data (hex): ");
  for (byte i=0; i<=FRAMESIZE-1; i++) {
    Serial.print(data[i],HEX);
    Serial.print(" ");
  }
  Serial.println(); 
}

//********************************************************
//!  turns LED on and off
//!
//!  returns nothing
//********************************************************
void ledOn() {  
  digitalWrite(LED_PIN,1);  
}
void ledOff() {  
  digitalWrite(LED_PIN,0);  
}
void toggleLED() {
  static boolean ledStatus = true;
  if (ledStatus) {
    ledOff();
    ledStatus = false;
  }
  else {
    ledOn();
    ledStatus = true;
  }
}

//********************************************************
//!  8-bit CRC 0x97 appropriate for short messages with HD-4
//!  http://hacromatic.com/blog/2012/08/make-your-serial-protocol-robust-with-8-bit-crc-codes/
//!
//!  returns resultant crc byte
//********************************************************
byte crc_update(byte crc, const unsigned char *data, size_t data_len) {
  unsigned int tbl_idx;

  while (data_len--) {
    tbl_idx = ((crc >> 0) ^ *data) & 0xFF;
    crc = (pgm_read_byte(crc_table+tbl_idx) ^ (crc << 8)) & 0xFF;
    data++;
  }
  return (crc & 0xFF);
}
danstrider is online now Find More Posts by danstrider
Last edited by danstrider; Aug 29, 2013 at 10:05 PM.
Reply With Quote