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
12 years ago · 193902079c
4 changed files with 2389 additions and 2298 deletions
--- a/obj/baseflight.hex
+++ b/obj/baseflight.hex
--- a/src/cli.c
+++ b/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
 };

--- a/src/gps.c
+++ b/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",
--- a/src/telemetry.c
+++ b/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 (sensors(SENSOR_GPS)) {
+        if ((cycleNum % 4) == 0) {      // Sent every 500ms
+            sendBaro();
+            sendHeading();
+            sendTelemetryTail();
+        }
+
+        if ((cycleNum % 8) == 0) {      // Sent every 1s
+            sendTemperature1();
+            if (feature(FEATURE_VBAT))
+                sendVoltage();
+            if (sensors(SENSOR_GPS))
                sendGPS();
-                sendTelemetryTail();
-            }
+            sendTelemetryTail();
        }

-        if (cycleNum == 25) {     //Frame 3: Sent every 5s
+        if (cycleNum == 40) {     //Frame 3: Sent every 5s
            cycleNum = 0;
            sendTime();
            sendTelemetryTail();