Diff: telemetry.c

Revision:

1:1e3318a30ddd

Parent:

0:62a1c91a859a

Child:

2:90292f8bd179

--- a/telemetry.c	Fri Feb 18 22:28:05 2011 +0000
+++ b/telemetry.c	Fri Feb 25 01:35:24 2011 +0000
@@ -34,7 +34,8 @@
 void SendNavPacket(void);
 void SendControlPacket(void);
 void SendStatsPacket(void);
-void SendParamPacket(uint8);
+void SendParamPacket(uint8, uint8);
+void SendParameters(uint8);
 void SendMinPacket(void);
 void SendArduStation(void);
 void SendCustom(void);
@@ -219,7 +220,7 @@
     SendPacketHeader();
 
     TxESCu8(UAVXFlightPacketTag);
-    TxESCu8(48 + TELEMETRY_FLAG_BYTES);
+    TxESCu8(58 + TELEMETRY_FLAG_BYTES);
     for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
         TxESCu8(F.AllFlags[b]);
 
@@ -240,11 +241,8 @@
     TxESCi8((int8)Comp[UD]);
     TxESCi8((int8)Comp[Alt]);
 
-    for ( b = 0; b < 4; b++ )
-        TxESCu8((uint8)PWM[b]);
-
-    TxESCu8((uint8)PWM[CamRollC]);
-    TxESCu8((uint8)PWM[CamPitchC]);
+    for ( b = 0; b < 8; b++ )
+        TxESCi16((int16)PWM[b]);
 
     TxESCi24(mSClock() - mS[StartTime]);
 
@@ -257,14 +255,16 @@
     SendPacketHeader();
 
     TxESCu8(UAVXControlPacketTag);
-    TxESCu8(35);
+    TxESCu8(46);
 
     TxESCi16(DesiredThrottle);
 
     ShowAttitude();
 
-    for ( b = 0; b < 6; b++ ) // motor/servo channels
-        TxESCu8((uint8)PWM[b]);
+    TxESCu8(UAVXAirframe | 0x80);
+
+    for ( b = 0; b < 8; b++ ) // motor/servo channels
+        TxESCi16((int16)PWM[b]);
 
     TxESCi24( time(NULL) );
 
@@ -320,6 +320,20 @@
 
 } // SendNavPacket
 
+void SendStickPacket(void) {
+    static uint8 c;
+
+    SendPacketHeader();
+
+    TxESCu8(UAVXStickPacketTag);
+    TxESCu8( 1 + RC_CONTROLS * 2 );
+    TxESCu8(RC_CONTROLS);
+    for ( c = 0; c < RC_CONTROLS; c++ )
+        TxESCi16(RC[c]);
+
+    SendPacketTrailer();
+} // SendStickPacket
+
 void SendStatsPacket(void) {
     SendPacketHeader();
 
@@ -373,7 +387,7 @@
     TxESCu8(State);
     TxESCu8(NavState);
     TxESCu8(FailState);
-    
+
     TxESCi16(BatteryVolts * 10.0); // to do scaling
     TxESCi16(BatteryCurrent * 10.0);
     TxESCi16(BatteryChargeUsedAH * 1000.0);
@@ -398,23 +412,33 @@
 
 } // SendMinPacket
 
-void SendParamPacket(uint8 p) {
-
-    static uint8 b;
+void SendParamPacket(uint8 s, uint8 p) {
 
     SendPacketHeader();
+    static union { real32 r32;
+        int32 i32;
+    } Temp;
 
-    TxESCu8(UAVXParamsPacketTag);
-    TxESCu8(MAX_PARAMETERS+1);
+    //  Temp.r32 = K[p];
+
+    TxESCu8(UAVXArmParamPacketTag);
+    TxESCu8(6);
+    TxESCu8(s);
     TxESCu8(p);
-    for (b = 0; b < (uint8)MAX_PARAMETERS; b++ )
-        TxESCi8(PX[MAX_PARAMETERS*2 + b]);
-
+    TxESCi32(K[p] * 1000.0 );
     SendPacketTrailer();
 
 } // SendParamPacket
 
-void SendCycle(void) { 
+void SendParameters(uint8 s) {
+    static uint8 p;
+
+    for ( p = 0; p < MAX_PARAMETERS; p++ )
+        SendParamPacket(s, p);
+    SendParamPacket(0, MAX_PARAMETERS);
+} // SendParameters
+
+void SendCycle(void) {
 
     switch ( UAVXCurrPacketTag ) {
         case UAVXFlightPacketTag:
@@ -467,7 +491,7 @@
     SST: Switch Status, used for debugging, but is disabled in the current version.
     */
 
-    if ( ++Count == 4 ) { 
+    if ( ++Count == 4 ) {
         TxString("!!!");
         TxString("LAT:");
         TxVal32(GPSLatitude / 6000, 3, 0);
@@ -511,8 +535,7 @@
 
 } // SendArduStation
 
-void SendCustom(void)
-{ // user defined telemetry human readable OK for small amounts of data < 1mS
+void SendCustom(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
 
     EchoToLogFile = true;