moved some readonly strings to flash, thanks zyp

cleaned up frsky telemetry a bit with verbose telemetry ID names, changed loop timing a bit, added output for heading and voltage (when vbat is enabled). telemetry stuff is untested, so someone try it out.

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

src/cli.c

@@ -30,7 +30,7 @@ static float _atof(const char *p);
 static char *ftoa(float x, char *floatString);
 
 // sync this with MultiType enum from mw.h
-const char *mixerNames[] = {
+const char * const mixerNames[] = {
     "TRI", "QUADP", "QUADX", "BI",
     "GIMBAL", "Y6", "HEX6",
     "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
@@ -38,7 +38,7 @@ const char *mixerNames[] = {
 };
 
 // sync this with AvailableFeatures enum from board.h
-const char *featureNames[] = {
+const char * const featureNames[] = {
     "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
     "SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
     "FAILSAFE", "SONAR", "TELEMETRY",
@@ -46,12 +46,12 @@ const char *featureNames[] = {
 };
 
 // sync this with AvailableSensors enum from board.h
-const char *sensorNames[] = {
+const char * const sensorNames[] = {
     "ACC", "BARO", "MAG", "SONAR", "GPS", NULL
 };
 
 // 
-const char *accNames[] = {
+const char * const accNames[] = {
     "", "ADXL345", "MPU6050", "MMA845x", NULL
 };
 

src/gps.c

@@ -5,13 +5,13 @@
 #define sq(x) ((x)*(x))
 #endif
 
-uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
+const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
 
 static void GPS_NewData(uint16_t c);
 static void GPS_set_pids(void);
 static void gpsPrint(const char *str);
 
-static const char *gpsInitStrings[] = {
+static const char * const gpsInitStrings[] = {
     "$PUBX,41,1,0003,0001,19200,0*23\r\n",      // UBX0..3
     "$PUBX,41,1,0003,0001,38400,0*26\r\n",
     "$PUBX,41,1,0003,0001,57600,0*2D\r\n",

src/telemetry.c

@@ -4,49 +4,101 @@
 #include "board.h"
 #include "mw.h"
 
-#define CYCLETIME       200
+#define CYCLETIME             125
+
+#define PROTOCOL_HEADER       0x5E
+#define PROTOCOL_TAIL         0x5E
+
+// Data Ids  (bp = before decimal point; af = after decimal point)
+// Official data IDs
+#define ID_GPS_ALTIDUTE_BP    0x01
+#define ID_GPS_ALTIDUTE_AP    0x09
+#define ID_TEMPRATURE1        0x02
+#define ID_RPM                0x03
+#define ID_FUEL_LEVEL         0x04
+#define ID_TEMPRATURE2        0x05
+#define ID_VOLT               0x06
+#define ID_ALTITUDE_BP        0x10
+#define ID_ALTITUDE_AP        0x21
+#define ID_GPS_SPEED_BP       0x11
+#define ID_GPS_SPEED_AP       0x19
+#define ID_LONGITUDE_BP       0x12
+#define ID_LONGITUDE_AP       0x1A
+#define ID_E_W                0x22
+#define ID_LATITUDE_BP        0x13
+#define ID_LATITUDE_AP        0x1B
+#define ID_N_S                0x23
+#define ID_COURSE_BP          0x14
+#define ID_COURSE_AP          0x1C
+#define ID_DATE_MONTH         0x15
+#define ID_YEAR               0x16
+#define ID_HOUR_MINUTE        0x17
+#define ID_SECOND             0x18
+#define ID_ACC_X              0x24
+#define ID_ACC_Y              0x25
+#define ID_ACC_Z              0x26
+#define ID_VOLTAGE_AMP_BP     0x3A
+#define ID_VOLTAGE_AMP_AP     0x3B
+#define ID_CURRENT            0x28
+// User defined data IDs
+#define ID_GYRO_X             0x40
+#define ID_GYRO_Y             0x41
+#define ID_GYRO_Z             0x42
 
 static void sendDataHead(uint8_t id)
 {
-    uartWrite(0x5E);
+    uartWrite(PROTOCOL_HEADER);
     uartWrite(id);
 }
 
 static void sendTelemetryTail(void)
 {
-    uartWrite(0x5E);
+    uartWrite(PROTOCOL_TAIL);
+}
+
+static void serializeFrsky(uint8_t data)
+{
+    // take care of byte stuffing
+    if (data == 0x5e) {
+        uartWrite(0x5d);
+        uartWrite(0x3e);
+    } else if (data == 0x5d) {
+        uartWrite(0x5d);
+        uartWrite(0x3d);
+    } else
+        uartWrite(data);
 }
 
 static void serialize16(int16_t a)
 {
     uint8_t t;
     t = a;
-    uartWrite(t);
+    serializeFrsky(t);
     t = a >> 8 & 0xff;
-    uartWrite(t);
+    serializeFrsky(t);
 }
 
 static void sendAccel(void)
 {
-    uint8_t i;
+    int i;
 
     for (i = 0; i < 3; i++) {
-        sendDataHead(0x24 + i);
+        sendDataHead(ID_ACC_X + i);
         serialize16(((float)accSmooth[i] / acc_1G) * 1000);
     }
 }
 
 static void sendBaro(void)
 {
-    sendDataHead(0x10);
+    sendDataHead(ID_ALTITUDE_BP);
     serialize16(EstAlt / 100);
-    sendDataHead(0x21);
+    sendDataHead(ID_ALTITUDE_AP);
     serialize16(EstAlt % 100);
 }
 
 static void sendTemperature1(void)
 {
-    sendDataHead(0x02);
+    sendDataHead(ID_TEMPRATURE1);
     serialize16(telemTemperature1 / 10);
 }
 
@@ -56,30 +108,50 @@ static void sendTime(void)
     uint8_t minutes = (seconds / 60) % 60;
 
     // if we fly for more than an hour, something's wrong anyway
-    sendDataHead(0x17);
+    sendDataHead(ID_HOUR_MINUTE);
     serialize16(minutes << 8);
-    sendDataHead(0x18);
+    sendDataHead(ID_SECOND);
     serialize16(seconds % 60);
 }
 
 static void sendGPS(void)
 {
-    sendDataHead(0x13);
+    sendDataHead(ID_LATITUDE_BP);
     serialize16(abs(GPS_coord[LAT]) / 100000);
-    sendDataHead(0x13 + 8);
+    sendDataHead(ID_LATITUDE_AP);
     serialize16((abs(GPS_coord[LAT]) / 100000) % 10000);
 
-    sendDataHead(0x1B + 8);
+    sendDataHead(ID_N_S);
     serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
 
-    sendDataHead(0x12);
+    sendDataHead(ID_LONGITUDE_BP);
     serialize16(abs(GPS_coord[LON]) / 100000);
-    sendDataHead(0x12 + 8);
+    sendDataHead(ID_LONGITUDE_AP);
     serialize16((abs(GPS_coord[LON]) / 100000) % 10000);
-    sendDataHead(0x1A + 8);
+    sendDataHead(ID_E_W);
     serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
 }
 
+static void sendVoltage(void)
+{
+    uint16_t voltage;
+
+    voltage = (vbat * 110) / 21;
+
+    sendDataHead(ID_VOLTAGE_AMP_BP);
+    serialize16(voltage / 100);
+    sendDataHead(ID_VOLTAGE_AMP_AP);
+    serialize16(((voltage % 100) + 5) / 10);
+}
+
+static void sendHeading(void)
+{
+    sendDataHead(ID_COURSE_BP);
+    serialize16(heading);
+    sendDataHead(ID_COURSE_AP);
+    serialize16(0);
+}
+
 static bool telemetryEnabled = false;
 
 void initTelemetry(bool State)
@@ -102,20 +174,26 @@ void sendTelemetry(void)
         lastCycleTime = millis();
         cycleNum++;
 
-        // Frame 1: sent every 200ms
+        // Sent every 125ms
         sendAccel();
-        sendBaro();
-        sendTemperature1();
         sendTelemetryTail();
 
-        if ((cycleNum % 5) == 0) {      // Frame 2: Sent every 1s
+        if ((cycleNum % 4) == 0) {      // Sent every 500ms
-            if (sensors(SENSOR_GPS)) {
+            sendBaro();
+            sendHeading();
+            sendTelemetryTail();
+        }
+
+        if ((cycleNum % 8) == 0) {      // Sent every 1s
+            sendTemperature1();
+            if (feature(FEATURE_VBAT))
+                sendVoltage();
+            if (sensors(SENSOR_GPS))
                 sendGPS();
             sendTelemetryTail();
         }
-        }
 
-        if (cycleNum == 25) {     //Frame 3: Sent every 5s
+        if (cycleNum == 40) {     //Frame 3: Sent every 5s
             cycleNum = 0;
             sendTime();
             sendTelemetryTail();