Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: telemetry.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/telemetry.c Fri Feb 25 01:35:24 2011 +0000
+++ b/telemetry.c Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
// = UAVXArm Quadrocopter Controller =
// = Copyright (c) 2008 by Prof. Greg Egan =
// = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
-// = http://code.google.com/p/uavp-mods/ http://uavp.ch =
+// = http://code.google.com/p/uavp-mods/ =
// ===============================================================================================
// This is part of UAVXArm.
@@ -38,6 +38,7 @@
void SendParameters(uint8);
void SendMinPacket(void);
void SendArduStation(void);
+void SendPIDTuning(void);
void SendCustom(void);
void SensorTrace(void);
@@ -161,7 +162,7 @@
break;
case CustomTelemetry:
mS[TelemetryUpdate] = mSClock() + CUSTOM_TEL_INTERVAL_MS;
- SendCustom();
+ SendPIDTuning();//SendCustom();
break;
case GPSTelemetry:
break;
@@ -170,15 +171,9 @@
} // CheckTelemetry
void SendPacketHeader(void) {
- static int8 b;
EchoToLogFile = true;
-#ifdef TELEMETRY_PREAMBLE
- for (b=10;b;b--)
- TxChar(0x55);
-#endif // TELEMETRY_PREAMBLE
-
TxChar(0xff); // synchronisation to "jolt" USART
TxChar(SOH);
TxCheckSum = 0;
@@ -212,6 +207,7 @@
TxESCi16(Acc[LR] * 1000.0);
TxESCi16(Acc[BF] * 1000.0);
TxESCi16(Acc[UD] * 1000.0);
+
} // ShowAttitude
void SendFlightPacket(void) {
@@ -236,10 +232,10 @@
ShowAttitude();
- TxESCi8((int8)Comp[LR]);
- TxESCi8((int8)Comp[BF]);
- TxESCi8((int8)Comp[UD]);
- TxESCi8((int8)Comp[Alt]);
+ TxESCi8((int8)Correction[LR]);
+ TxESCi8((int8)Correction[BF]);
+ TxESCi8((int8)AccAltComp);
+ TxESCi8((int8)AltComp);
for ( b = 0; b < 8; b++ )
TxESCi16((int16)PWM[b]);
@@ -255,7 +251,7 @@
SendPacketHeader();
TxESCu8(UAVXControlPacketTag);
- TxESCu8(46);
+ TxESCu8(48);
TxESCi16(DesiredThrottle);
@@ -335,40 +331,19 @@
} // SendStickPacket
void SendStatsPacket(void) {
+
+ static uint8 i;
+
SendPacketHeader();
TxESCu8(UAVXStatsPacketTag);
- TxESCu8(44);
-
- TxESCi16(Stats[I2CFailS]);
- TxESCi16(Stats[GPSInvalidS]);
- TxESCi16(Stats[AccFailS]);
- TxESCi16(Stats[GyroFailS]);
- TxESCi16(Stats[CompassFailS]);
- TxESCi16(Stats[BaroFailS]);
- TxESCi16(Stats[ESCI2CFailS]);
-
- TxESCi16(Stats[RCFailsafesS]);
+ TxESCu8(MAX_STATS * 2 + 2);
- TxESCi16(Stats[GPSAltitudeS]);
- TxESCi16(Stats[GPSVelS]);
- TxESCi16(Stats[GPSMinSatsS]);
- TxESCi16(Stats[GPSMaxSatsS]);
- TxESCi16(Stats[MinHDiluteS]);
- TxESCi16(Stats[MaxHDiluteS]);
-
- TxESCi16(Stats[BaroRelAltitudeS]);
- TxESCi16(0);//Stats[MinBaroROCS]);
- TxESCi16(0);//Stats[MaxBaroROCS]);
-
- TxESCi16(Stats[MinTempS]);
- TxESCi16(Stats[MaxTempS]);
-
- TxESCi16(Stats[BadS]);
+ for ( i = 0; i < MAX_STATS ; i++)
+ TxESCi16(Stats[i]);
TxESCu8(UAVXAirframe | 0x80);
TxESCu8(Orientation);
- TxESCi16(Stats[BadNumS]);
SendPacketTrailer();
@@ -415,17 +390,13 @@
void SendParamPacket(uint8 s, uint8 p) {
SendPacketHeader();
- static union { real32 r32;
- int32 i32;
- } Temp;
-
- // Temp.r32 = K[p];
TxESCu8(UAVXArmParamPacketTag);
TxESCu8(6);
TxESCu8(s);
TxESCu8(p);
TxESCi32(K[p] * 1000.0 );
+
SendPacketTrailer();
} // SendParamPacket
@@ -436,6 +407,7 @@
for ( p = 0; p < MAX_PARAMETERS; p++ )
SendParamPacket(s, p);
SendParamPacket(0, MAX_PARAMETERS);
+
} // SendParameters
void SendCycle(void) {
@@ -535,45 +507,58 @@
} // SendArduStation
+void SendPIDTuning(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
+
+ // Fixed to roll axis
+
+ SendPacketHeader();
+
+ TxESCu8(UAVXCustomPacketTag);
+ TxESCu8(1 + 10);
+ TxESCu8(5); // how many 16bit elements
+/*
+ TxESCi16(DesiredRoll);
+ TxESCi16(PWM[RightC]);
+
+ TxESCi16(Gyro[Roll] * 1000.0);
+ TxESCi16(Acc[Roll] * 1000.0);
+ TxESCi16(Angle[Roll] * 1000.0 );
+ */
+
+ TxESCi16(DesiredYaw);
+ TxESCi16(PWM[RightC]);
+
+ TxESCi16(Gyro[Yaw] * 1000.0);
+ TxESCi16(HE * 1000.0);
+ TxESCi16(Angle[Yaw] * 1000.0 );
+
+ SendPacketTrailer();
+
+} // SendPIDTuning
+
+
void SendCustom(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
- EchoToLogFile = true;
+ static uint8 s;
- // insert values here using TxVal32(n, dp, separator)
- // dp is the scaling to decimal places, separator
- // separator may be a single 'char', HT for tab, or 0 (no space)
- // ->
+ // always a vector of int16;
+ SendPacketHeader();
- TxVal32(mSClock(), 3, HT);
-
- if ( F.HoldingAlt ) // are we holding
- TxChar('H');
- else
- TxChar('N');
- TxChar(HT);
+ TxESCu8(UAVXCustomPacketTag);
+ TxESCu8(1 + 8 + MaxAttitudeScheme * 2);
+ TxESCu8(4 + MaxAttitudeScheme ); // how many 16bit elements
+ TxESCi16(AttitudeMethod);
- if (F.UsingRangefinderAlt ) // are we using the rangefinder
- TxChar('R');
- else
- TxChar('B');
- TxChar(HT);
+ TxESCi16(0); // spare
- TxVal32(SRS32(Comp[Alt],1), 1, HT); // ~% throttle compensation
+ TxESCi16(Gyro[Roll] * 1000.0);
+ TxESCi16(Acc[LR] * 1000.0);
- TxVal32(GPSRelAltitude, 1, HT);
- TxVal32(BaroRelAltitude, 1, HT);
- TxVal32(RangefinderAltitude, 2, HT);
+ for ( s = 0; s < MaxAttitudeScheme; s++ )
+ TxESCi16( EstAngle[Roll][s] * 1000.0 );
- TxVal32(BaroPressure, 0, HT); // eff. sensor reading
- TxVal32(BaroTemperature, 0, HT); // eff. sensor reading redundant for MPX4115
- TxVal32(CompBaroPressure, 0, HT); // moving sum of last 8 readings
+ SendPacketTrailer();
- // <-
-
- TxChar(CR);
- TxChar(LF);
-
- EchoToLogFile = false;
} // SendCustom
void SensorTrace(void) {
@@ -622,13 +607,13 @@
TxValH16(Acc[DU]);
TxChar(';');
- TxValH16(Comp[LR]);
+ TxValH16(Correction[LR]);
TxChar(';');
- TxValH16(Comp[FB]);
+ TxValH16(Correction[FB]);
TxChar(';');
- TxValH16(Comp[DU]);
+ TxValH16(AccAltComp);
TxChar(';');
- TxValH16(Comp[Alt]);
+ TxValH16(AltComp);
TxChar(';');
TxNextLine();
}