added support for outputting FrSky telemetry by silpstream @ rcgroups

feature TELEMETRY to enable it. when armed, telemetry is sent out the TX pin. It must be connected to 'Rx' pin of FrSky telemetry receiver via a level shifter. When disarmed, port goes back to 115200 bps for GUI.
fixed a typo in imu.c introduced by  GYRO_INTERLEAVE junk.
todo: just switch serial speed instead of reinitializing everything.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@173 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

baseflight.uvproj

@@ -451,6 +451,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\buzzer.c</FilePath>
             </File>
+            <File>
+              <FileName>telemetry.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\telemetry.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1221,6 +1226,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\buzzer.c</FilePath>
             </File>
+            <File>
+              <FileName>telemetry.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\telemetry.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1937,6 +1947,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\buzzer.c</FilePath>
             </File>
+            <File>
+              <FileName>telemetry.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\telemetry.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

src/board.h

@@ -47,6 +47,7 @@ typedef enum {
     FEATURE_GPS = 1 << 9,
     FEATURE_FAILSAFE = 1 << 10,
     FEATURE_SONAR = 1 << 11,
+    FEATURE_TELEMETRY = 1 << 12,
 } AvailableFeatures;
 
 typedef void (* sensorInitFuncPtr)(void);                   // sensor init prototype
@@ -59,6 +60,7 @@ typedef struct sensor_t
     sensorInitFuncPtr init;
     sensorReadFuncPtr read;
     sensorReadFuncPtr align;
+    sensorReadFuncPtr temperature;
 } sensor_t;
 
 #define digitalHi(p, i)     { p->BSRR = i; }

src/cli.c

@@ -37,7 +37,7 @@ const char *mixerNames[] = {
 const char *featureNames[] = {
     "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
     "SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
-    "FAILSAFE", "SONAR",
+    "FAILSAFE", "SONAR", "TELEMETRY",
     NULL
 };
 

src/drv_mpu3050.c

@@ -29,6 +29,7 @@ static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
 static void mpu3050Init(void);
 static void mpu3050Read(int16_t *gyroData);
 static void mpu3050Align(int16_t *gyroData);
+static void mpu3050ReadTemp(int16_t *tempData);
 
 bool mpu3050Detect(sensor_t *gyro)
 {
@@ -43,6 +44,7 @@ bool mpu3050Detect(sensor_t *gyro)
     gyro->init = mpu3050Init;
     gyro->read = mpu3050Read;
     gyro->align = mpu3050Align;
+    gyro->temperature = mpu3050ReadTemp;
 
     return true;
 }
@@ -107,10 +109,10 @@ static void mpu3050Read(int16_t *gyroData)
     gyroData[2] = (buf[4] << 8) | buf[5];
 }
 
-static int16_t mpu3050ReadTemp(void)
+static void mpu3050ReadTemp(int16_t *tempData)
 {
     uint8_t buf[2];
     i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
-    
-    return 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
+
+    *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
 }

src/gps.c

@@ -619,10 +619,10 @@ static uint32_t GPS_coord_to_degrees(char *s)
 }
 
 // helper functions
-static uint16_t grab_fields(char *src, uint8_t mult)
-{                               // convert string to uint16
+static uint32_t grab_fields(char *src, uint8_t mult)
+{                               // convert string to uint32
     uint8_t i;
-    uint16_t tmp = 0;
+    uint32_t tmp = 0;
     for (i = 0; src[i] != 0; i++) {
         if (src[i] == '.') {
             i++;

src/imu.c

@@ -50,7 +50,7 @@ void computeIMU(void)
 
     Gyro_getADC();
 
-    for (axis = 0; axis < 3; axis++)
+    for (axis = 0; axis < 3; axis++) {
 #ifdef GYRO_INTERLEAVE
         gyroADCp[axis] = gyroADC[axis];
 #else
@@ -58,6 +58,7 @@ void computeIMU(void)
 #endif
         if (!sensors(SENSOR_ACC))
             accADC[axis] = 0;
+    }
     timeInterleave = micros();
     annexCode();
 #ifdef GYRO_INTERLEAVE

src/mw.c

@@ -14,6 +14,7 @@ int16_t headFreeModeHold;
 
 int16_t annex650_overrun_count = 0;
 uint8_t vbat;                   // battery voltage in 0.1V steps
+int16_t telemTemperature1;      // gyro sensor temperature
 
 int16_t failsafeCnt = 0;
 int16_t failsafeEvents = 0;
@@ -170,9 +171,11 @@ void annexCode(void)
         if (f.ACC_CALIBRATED) {
             LED0_OFF;
         }
-        if (f.ARMED) {
+        if (f.ARMED)
             LED0_ON;
-        }
+        // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
+        if (feature(FEATURE_TELEMETRY))
+            initTelemetry(f.ARMED);
     }
 
 #ifdef LEDRING
@@ -204,6 +207,13 @@ void annexCode(void)
             LED1_TOGGLE;
         }
     }
+
+    // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
+    if (gyro.temperature)
+        gyro.temperature(&telemTemperature1);
+    else {
+        // TODO MCU temp
+    }
 }
 
 uint16_t pwmReadRawRC(uint8_t chan)

src/mw.h

@@ -247,6 +247,7 @@ extern int16_t motor[8];
 extern int16_t servo[8];
 extern int16_t rcData[8];
 extern uint8_t vbat;
+extern int16_t telemTemperature1;      // gyro sensor temperature
 extern int16_t lookupPitchRollRC[6];   // lookup table for expo & RC rate PITCH+ROLL
 extern int16_t lookupThrottleRC[11];   // lookup table for expo & mid THROTTLE
 extern uint8_t toggleBeep;
@@ -335,3 +336,7 @@ void gpsInit(uint32_t baudrate);
 void GPS_reset_home_position(void);
 void GPS_reset_nav(void);
 void GPS_set_next_wp(int32_t* lat, int32_t* lon);
+
+// telemetry
+void initTelemetry(bool State);
+void sendTelemetry(void);

src/serial.c

@@ -70,6 +70,7 @@ static const char pidnames[] =
 
 static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
 static uint8_t cmdMSP;
+static bool guiConnected = false;
 
 void serialize32(uint32_t a)
 {
@@ -396,6 +397,7 @@ void serialCom(void)
             indRX = 0;
             checksum ^= c;
             c_state = HEADER_SIZE;      // the command is to follow
+            guiConnected = true;
         } else if (c_state == HEADER_SIZE) {
             cmdMSP = c;
             checksum ^= c;
@@ -410,4 +412,8 @@ void serialCom(void)
             c_state = IDLE;
         }
     }
+    if (!cliMode && !uartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { //The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
+        sendTelemetry();
+        return;
+    }
 }

src/telemetry.c

@@ -0,0 +1,124 @@
+/* 
+ * FrSky Telemetry implementation by silpstream @ rcgroups
+ */
+#include "board.h"
+#include "mw.h"
+
+#define CYCLETIME       200
+
+static void sendDataHead(uint8_t id)
+{
+    uartWrite(0x5E);
+    uartWrite(id);
+}
+
+static void sendTelemetryTail(void)
+{
+    uartWrite(0x5E);
+}
+
+static void serialize16(int16_t a)
+{
+    uint8_t t;
+    t = a;
+    uartWrite(t);
+    t = a >> 8 & 0xff;
+    uartWrite(t);
+}
+
+static void sendAccel(void)
+{
+    uint8_t i;
+
+    for (i = 0; i < 3; i++) {
+        sendDataHead(0x24 + i);
+        serialize16(((float)accSmooth[i] / acc_1G) * 1000);
+    }
+}
+
+static void sendBaro(void)
+{
+    sendDataHead(0x10);
+    serialize16(EstAlt / 100);
+    sendDataHead(0x21);
+    serialize16(EstAlt % 100);
+}
+
+static void sendTemperature1(void)
+{
+    sendDataHead(0x02);
+    serialize16(telemTemperature1 / 10);
+}
+
+static void sendTime(void)
+{
+    uint32_t seconds = millis() / 1000;
+    uint8_t minutes = (seconds / 60) % 60;
+
+    // if we fly for more than an hour, something's wrong anyway
+    sendDataHead(0x17);
+    serialize16(minutes << 8);
+    sendDataHead(0x18);
+    serialize16(seconds % 60);
+}
+
+static void sendGPS(void)
+{
+    sendDataHead(0x13);
+    serialize16(abs(GPS_coord[LAT]) / 100000);
+    sendDataHead(0x13 + 8);
+    serialize16((abs(GPS_coord[LAT]) / 100000) % 10000);
+
+    sendDataHead(0x1B + 8);
+    serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
+
+    sendDataHead(0x12);
+    serialize16(abs(GPS_coord[LON]) / 100000);
+    sendDataHead(0x12 + 8);
+    serialize16((abs(GPS_coord[LON]) / 100000) % 10000);
+    sendDataHead(0x1A + 8);
+    serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
+}
+
+static bool telemetryEnabled = false;
+
+void initTelemetry(bool State)
+{
+    if (State != telemetryEnabled) {
+        if (State)
+            serialInit(9600);
+        else
+            serialInit(cfg.serial_baudrate);
+        telemetryEnabled = State;
+    }
+}
+
+static uint32_t lastCycleTime = 0;
+static uint8_t cycleNum = 0;
+
+void sendTelemetry(void)
+{
+    if (millis() - lastCycleTime >= CYCLETIME) {
+        lastCycleTime = millis();
+        cycleNum++;
+
+        // Frame 1: sent every 200ms
+        sendAccel();
+        sendBaro();
+        sendTemperature1();
+        sendTelemetryTail();
+
+        if ((cycleNum % 5) == 0) {      // Frame 2: Sent every 1s
+            if (sensors(SENSOR_GPS)) {
+                sendGPS();
+                sendTelemetryTail();
+            }
+        }
+
+        if (cycleNum == 25) {     //Frame 3: Sent every 5s
+            cycleNum = 0;
+            sendTime();
+            sendTelemetryTail();
+        }
+    }
+}