mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-19 04:52:12 +01:00
Merge remote-tracking branch 'origin/amorale/OP-1750_perf_optimization' into next
This commit is contained in:
Alessio Morale
commit
59325f6292
@ -33,25 +33,26 @@
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <pios_math.h>
|
||||
#include <mathmisc.h>
|
||||
|
||||
// constants/macros/typdefs
|
||||
#define NUMX 13 // number of states, X is the state vector
|
||||
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
#pragma GCC optimize "O3"
|
||||
// Private functions
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed);
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed);
|
||||
static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
// Private variables
|
||||
|
||||
@ -60,29 +61,29 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
// LinearizeFG() and LinearizeH():
|
||||
//
|
||||
// usage F: usage G: usage H:
|
||||
// 0123456789abc 012345678 0123456789abc
|
||||
// -0123456789abc 012345678 0123456789abc
|
||||
// 0...X......... ......... X............
|
||||
// 1....X........ ......... .X...........
|
||||
// 2.....X....... ......... ..X..........
|
||||
// 3......XXXX... ...XXX... ...X.........
|
||||
// 4......XXXX... ...XXX... ....X........
|
||||
// 5......XXXX... ...XXX... .....X.......
|
||||
// 6.......XXXXXX XXX...... ......XXXX...
|
||||
// 7......X.XXXXX XXX...... ......XXXX...
|
||||
// 8......XX.XXXX XXX...... ......XXXX...
|
||||
// 9......XXX.XXX XXX...... ..X..........
|
||||
// a............. ......X..
|
||||
// b............. .......X.
|
||||
// c............. ........X
|
||||
// 6.....ooXXXXXX XXX...... ......XXXX...
|
||||
// 7.....oXoXXXXX XXX...... ......XXXX...
|
||||
// 8.....oXXoXXXX XXX...... ......XXXX...
|
||||
// 9.....oXXXoXXX XXX...... ..X..........
|
||||
// a............. ......Xoo
|
||||
// b............. ......oXo
|
||||
// c............. ......ooX
|
||||
|
||||
static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 };
|
||||
static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
|
||||
static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
|
||||
static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
|
||||
|
||||
static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
|
||||
static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
|
||||
static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
|
||||
static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
|
||||
|
||||
static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
|
||||
static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
|
||||
static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
|
||||
static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
|
||||
|
||||
static struct EKFData {
|
||||
// linearized system matrices
|
||||
@ -182,8 +183,8 @@ void INSGetP(float PDiag[NUMX])
|
||||
uint8_t i;
|
||||
|
||||
// retrieve diagonal elements (aka state variance)
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
if (PDiag != 0) {
|
||||
if (PDiag != 0) {
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
PDiag[i] = ekf.P[i][i];
|
||||
}
|
||||
}
|
||||
@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3])
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
float qmag;
|
||||
float invqmag;
|
||||
|
||||
// rate gyro inputs in units of rad/s
|
||||
U[0] = gyro_data[0];
|
||||
@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
// EKF prediction step
|
||||
LinearizeFG(ekf.X, U, ekf.F, ekf.G);
|
||||
RungeKutta(ekf.X, U, dT);
|
||||
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] /= qmag;
|
||||
ekf.X[7] /= qmag;
|
||||
ekf.X[8] /= qmag;
|
||||
ekf.X[9] /= qmag;
|
||||
invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] *= invqmag;
|
||||
ekf.X[7] *= invqmag;
|
||||
ekf.X[8] *= invqmag;
|
||||
ekf.X[9] *= invqmag;
|
||||
// CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
|
||||
|
||||
// Update Nav solution structure
|
||||
@ -373,8 +374,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt)
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt, uint16_t SensorsUsed)
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float Bmag, qmag;
|
||||
float Z[10] = { 0 };
|
||||
float Y[10] = { 0 };
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0] = Pos[0];
|
||||
@ -385,14 +386,15 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
Z[3] = Vel[0];
|
||||
Z[4] = Vel[1];
|
||||
Z[5] = Vel[2];
|
||||
|
||||
// magnetometer data in any units (use unit vector) and in body frame
|
||||
Bmag =
|
||||
sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
|
||||
mag_data[2] * mag_data[2]);
|
||||
Z[6] = mag_data[0] / Bmag;
|
||||
Z[7] = mag_data[1] / Bmag;
|
||||
Z[8] = mag_data[2] / Bmag;
|
||||
|
||||
|
||||
if (SensorsUsed & MAG_SENSORS) {
|
||||
float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
|
||||
Z[6] = mag_data[0] * invBmag;
|
||||
Z[7] = mag_data[1] * invBmag;
|
||||
Z[8] = mag_data[2] * invBmag;
|
||||
}
|
||||
|
||||
// barometric altimeter in meters and in local NED frame
|
||||
Z[9] = BaroAlt;
|
||||
@ -401,12 +403,12 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
LinearizeH(ekf.X, ekf.Be, ekf.H);
|
||||
MeasurementEq(ekf.X, ekf.Be, Y);
|
||||
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
|
||||
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] /= qmag;
|
||||
ekf.X[7] /= qmag;
|
||||
ekf.X[8] /= qmag;
|
||||
ekf.X[9] /= qmag;
|
||||
|
||||
float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] *= invqmag;
|
||||
ekf.X[7] *= invqmag;
|
||||
ekf.X[8] *= invqmag;
|
||||
ekf.X[9] *= invqmag;
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = ekf.X[0];
|
||||
Nav.Pos[1] = ekf.X[1];
|
||||
@ -434,60 +436,78 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
// The first Method is very specific to this implementation
|
||||
// ************************************************
|
||||
|
||||
__attribute__((optimize("O3")))
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
|
||||
|
||||
float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
|
||||
float dTsq = dT * dT;
|
||||
const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
|
||||
const float dTsq = dT * dT;
|
||||
|
||||
float Dummy[NUMX][NUMX];
|
||||
int8_t i;
|
||||
int8_t k;
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
|
||||
float *Firow = F[i];
|
||||
float *Pirow = P[i];
|
||||
float *Dirow = Dummy[i];
|
||||
int8_t Fistart = FrowMin[i];
|
||||
int8_t Fiend = FrowMax[i];
|
||||
float *Firow = F[i];
|
||||
float *Pirow = P[i];
|
||||
float *Dirow = Dummy[i];
|
||||
const int8_t Fistart = FrowMin[i];
|
||||
const int8_t Fiend = FrowMax[i];
|
||||
int8_t j;
|
||||
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
|
||||
int8_t k;
|
||||
for (k = Fistart; k <= Fiend; k++) {
|
||||
}
|
||||
for (k = Fistart; k <= Fiend; k++) {
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
|
||||
float *Dirow = Dummy[i];
|
||||
float *Girow = G[i];
|
||||
float *Pirow = P[i];
|
||||
int8_t Gistart = GrowMin[i];
|
||||
int8_t Giend = GrowMax[i];
|
||||
float *Dirow = Dummy[i];
|
||||
float *Girow = G[i];
|
||||
float *Pirow = P[i];
|
||||
const int8_t Gistart = GrowMin[i];
|
||||
const int8_t Giend = GrowMax[i];
|
||||
int8_t j;
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
|
||||
|
||||
{
|
||||
float *Fjrow = F[j];
|
||||
int8_t Fjstart = FrowMin[j];
|
||||
int8_t Fjend = FrowMax[j];
|
||||
int8_t k;
|
||||
for (k = Fjstart; k <= Fjend; k++) {
|
||||
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
|
||||
}
|
||||
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
|
||||
|
||||
const float *Fjrow = F[j];
|
||||
int8_t Fjstart = FrowMin[j];
|
||||
int8_t Fjend = FrowMax[j];
|
||||
k = Fjstart;
|
||||
|
||||
while (k <= Fjend - 3) {
|
||||
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
|
||||
Ptmp += Dirow[k + 1] * Fjrow[k + 1];
|
||||
Ptmp += Dirow[k + 2] * Fjrow[k + 2];
|
||||
Ptmp += Dirow[k + 3] * Fjrow[k + 3];
|
||||
k += 4;
|
||||
}
|
||||
while (k <= Fjend) {
|
||||
Ptmp += Dirow[k] * Fjrow[k];
|
||||
k++;
|
||||
}
|
||||
|
||||
{
|
||||
float *Gjrow = G[j];
|
||||
int8_t Gjstart = MAX(Gistart, GrowMin[j]);
|
||||
int8_t Gjend = MIN(Giend, GrowMax[j]);
|
||||
int8_t k;
|
||||
for (k = Gjstart; k <= Gjend; k++) {
|
||||
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
|
||||
float *Gjrow = G[j];
|
||||
const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
|
||||
const int8_t Gjend = MIN(Giend, GrowMax[j]);
|
||||
k = Gjstart;
|
||||
while (k <= Gjend - 2) {
|
||||
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
|
||||
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
|
||||
Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
|
||||
k += 3;
|
||||
}
|
||||
if (k <= Gjend) {
|
||||
Ptmp += Q[k] * Girow[k] * Gjrow[k];
|
||||
if (k <= Gjend - 1) {
|
||||
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
|
||||
}
|
||||
}
|
||||
|
||||
@ -511,7 +531,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
// The SensorsUsed variable is a bitwise mask indicating which sensors
|
||||
// should be used in the update.
|
||||
// ************************************************
|
||||
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed)
|
||||
@ -524,7 +543,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] = 0;
|
||||
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
|
||||
}
|
||||
|
||||
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] += H[m][k] * P[k][j];
|
||||
}
|
||||
}
|
||||
@ -532,14 +554,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
|
||||
HPHR += HP[k] * H[m][k];
|
||||
}
|
||||
|
||||
float invHPHR = 1.0f / HPHR;
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
Km[k] = HP[k] / HPHR; // find K = HP/HPHR
|
||||
Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
|
||||
for (j = i; j < NUMX; j++) {
|
||||
P[i][j] = P[j][i] =
|
||||
P[i][j] - Km[i] * HP[j];
|
||||
P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
|
||||
}
|
||||
}
|
||||
|
||||
@ -560,8 +581,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
{
|
||||
float dT2 =
|
||||
dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
const float dT2 = dT / 2.0f;
|
||||
float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
@ -585,7 +606,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
X[i] =
|
||||
Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
|
||||
K4[i]) / 6.0f;
|
||||
K4[i]) * (1.0f / 6.0f);
|
||||
}
|
||||
}
|
||||
|
||||
@ -608,7 +629,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
// H is output of LinearizeH(), all elements not set should be zero
|
||||
// ************************************************
|
||||
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
|
||||
static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
|
||||
@ -108,8 +108,8 @@ static void radioRxTask(void *parameters);
|
||||
static void PPMInputTask(void *parameters);
|
||||
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
|
||||
static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
|
||||
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
|
||||
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
|
||||
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
|
||||
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
|
||||
static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
|
||||
static void registerObject(UAVObjHandle obj);
|
||||
|
||||
@ -403,14 +403,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX);
|
||||
#endif
|
||||
if (PIOS_COM_RADIO) {
|
||||
uint8_t serial_data[1];
|
||||
uint8_t serial_data[16];
|
||||
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
|
||||
if (bytes_to_process > 0) {
|
||||
if (data->parseUAVTalk) {
|
||||
// Pass the data through the UAVTalk parser.
|
||||
for (uint8_t i = 0; i < bytes_to_process; i++) {
|
||||
ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]);
|
||||
}
|
||||
ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process);
|
||||
} else if (PIOS_COM_TELEMETRY) {
|
||||
// Send the data straight to the telemetry port.
|
||||
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
|
||||
@ -448,12 +446,10 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
if (inputPort) {
|
||||
uint8_t serial_data[1];
|
||||
uint8_t serial_data[16];
|
||||
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
|
||||
if (bytes_to_process > 0) {
|
||||
for (uint8_t i = 0; i < bytes_to_process; i++) {
|
||||
ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]);
|
||||
}
|
||||
ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process);
|
||||
}
|
||||
} else {
|
||||
vTaskDelay(5);
|
||||
@ -597,42 +593,45 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
|
||||
* @param[in] outConnectionHandle The UAVTalk connection handle on the radio port.
|
||||
* @param[in] rxbyte The received byte.
|
||||
*/
|
||||
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
|
||||
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
|
||||
{
|
||||
// Keep reading until we receive a completed packet.
|
||||
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
|
||||
uint8_t position = 0;
|
||||
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
// We only want to unpack certain telemetry objects
|
||||
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
|
||||
switch (objId) {
|
||||
case OPLINKSTATUS_OBJID:
|
||||
case OPLINKSETTINGS_OBJID:
|
||||
case OPLINKRECEIVER_OBJID:
|
||||
case MetaObjectId(OPLINKSTATUS_OBJID):
|
||||
case MetaObjectId(OPLINKSETTINGS_OBJID):
|
||||
case MetaObjectId(OPLINKRECEIVER_OBJID):
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
break;
|
||||
case OBJECTPERSISTENCE_OBJID:
|
||||
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
|
||||
// receive object locally
|
||||
// some objects will send back a response to telemetry
|
||||
// FIXME:
|
||||
// OPLM will ack or nack all objects requests and acked object sends
|
||||
// Receiver will probably also ack / nack the same messages
|
||||
// This has some consequences like :
|
||||
// Second ack/nack will not match an open transaction or will apply to wrong transaction
|
||||
// Question : how does GCS handle receiving the same object twice
|
||||
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
// relay packet to remote modem
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
default:
|
||||
// all other packets are relayed to the remote modem
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
// Keep reading until we receive a completed packet.
|
||||
while (position < length) {
|
||||
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
// We only want to unpack certain telemetry objects
|
||||
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
|
||||
switch (objId) {
|
||||
case OPLINKSTATUS_OBJID:
|
||||
case OPLINKSETTINGS_OBJID:
|
||||
case OPLINKRECEIVER_OBJID:
|
||||
case MetaObjectId(OPLINKSTATUS_OBJID):
|
||||
case MetaObjectId(OPLINKSETTINGS_OBJID):
|
||||
case MetaObjectId(OPLINKRECEIVER_OBJID):
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
break;
|
||||
case OBJECTPERSISTENCE_OBJID:
|
||||
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
|
||||
// receive object locally
|
||||
// some objects will send back a response to telemetry
|
||||
// FIXME:
|
||||
// OPLM will ack or nack all objects requests and acked object sends
|
||||
// Receiver will probably also ack / nack the same messages
|
||||
// This has some consequences like :
|
||||
// Second ack/nack will not match an open transaction or will apply to wrong transaction
|
||||
// Question : how does GCS handle receiving the same object twice
|
||||
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
// relay packet to remote modem
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
default:
|
||||
// all other packets are relayed to the remote modem
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -642,39 +641,43 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
|
||||
*
|
||||
* @param[in] inConnectionHandle The UAVTalk connection handle on the radio port.
|
||||
* @param[in] outConnectionHandle The UAVTalk connection handle on the telemetry port.
|
||||
* @param[in] rxbyte The received byte.
|
||||
* @param[in] rxbuffer The received buffer.
|
||||
* @param[in] length buffer length
|
||||
*/
|
||||
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
|
||||
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
|
||||
{
|
||||
// Keep reading until we receive a completed packet.
|
||||
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
|
||||
uint8_t position = 0;
|
||||
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
// We only want to unpack certain objects from the remote modem
|
||||
// Similarly we only want to relay certain objects to the telemetry port
|
||||
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
|
||||
switch (objId) {
|
||||
case OPLINKSTATUS_OBJID:
|
||||
case OPLINKSETTINGS_OBJID:
|
||||
case MetaObjectId(OPLINKSTATUS_OBJID):
|
||||
case MetaObjectId(OPLINKSETTINGS_OBJID):
|
||||
// Ignore object...
|
||||
// These objects are shadowed by the modem and are not transmitted to the telemetry port
|
||||
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
|
||||
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
|
||||
break;
|
||||
case OPLINKRECEIVER_OBJID:
|
||||
case MetaObjectId(OPLINKRECEIVER_OBJID):
|
||||
// Receive object locally
|
||||
// These objects are received by the modem and are not transmitted to the telemetry port
|
||||
// - OPLINKRECEIVER_OBJID : not sure why
|
||||
// some objects will send back a response to the remote modem
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
break;
|
||||
default:
|
||||
// all other packets are relayed to the telemetry port
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
// Keep reading until we receive a completed packet.
|
||||
while (position < length) {
|
||||
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
// We only want to unpack certain objects from the remote modem
|
||||
// Similarly we only want to relay certain objects to the telemetry port
|
||||
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
|
||||
switch (objId) {
|
||||
case OPLINKSTATUS_OBJID:
|
||||
case OPLINKSETTINGS_OBJID:
|
||||
case MetaObjectId(OPLINKSTATUS_OBJID):
|
||||
case MetaObjectId(OPLINKSETTINGS_OBJID):
|
||||
// Ignore object...
|
||||
// These objects are shadowed by the modem and are not transmitted to the telemetry port
|
||||
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
|
||||
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
|
||||
break;
|
||||
case OPLINKRECEIVER_OBJID:
|
||||
case MetaObjectId(OPLINKRECEIVER_OBJID):
|
||||
// Receive object locally
|
||||
// These objects are received by the modem and are not transmitted to the telemetry port
|
||||
// - OPLINKRECEIVER_OBJID : not sure why
|
||||
// some objects will send back a response to the remote modem
|
||||
UAVTalkReceiveObject(inConnectionHandle);
|
||||
break;
|
||||
default:
|
||||
// all other packets are relayed to the telemetry port
|
||||
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
|
||||
*/
|
||||
static void StateEstimationCb(void)
|
||||
{
|
||||
static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
|
||||
static filterResult alarm = FILTERRESULT_OK;
|
||||
static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
|
||||
static uint16_t alarmcounter = 0;
|
||||
@ -361,193 +360,171 @@ static void StateEstimationCb(void)
|
||||
return;
|
||||
}
|
||||
|
||||
switch (runState) {
|
||||
case RUNSTATE_LOAD:
|
||||
alarm = FILTERRESULT_OK;
|
||||
|
||||
alarm = FILTERRESULT_OK;
|
||||
|
||||
// set alarm to warning if called through timeout
|
||||
if (updatedSensors == 0) {
|
||||
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
|
||||
alarm = FILTERRESULT_WARNING;
|
||||
}
|
||||
} else {
|
||||
last_time = PIOS_DELAY_GetRaw();
|
||||
// set alarm to warning if called through timeout
|
||||
if (updatedSensors == 0) {
|
||||
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
|
||||
alarm = FILTERRESULT_WARNING;
|
||||
}
|
||||
} else {
|
||||
last_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
// check if a new filter chain should be initialized
|
||||
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
|
||||
FlightStatusData fs;
|
||||
FlightStatusGet(&fs);
|
||||
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
|
||||
const filterPipeline *newFilterChain;
|
||||
switch (revoSettings.FusionAlgorithm) {
|
||||
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
|
||||
newFilterChain = cfQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
|
||||
newFilterChain = cfmiQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
|
||||
newFilterChain = cfmQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
|
||||
newFilterChain = ekf13iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
|
||||
newFilterChain = ekf13Queue;
|
||||
break;
|
||||
default:
|
||||
newFilterChain = NULL;
|
||||
}
|
||||
// initialize filters in chain
|
||||
current = newFilterChain;
|
||||
bool error = 0;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->init((stateFilter *)current->filter);
|
||||
if (result != 0) {
|
||||
error = 1;
|
||||
break;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return;
|
||||
} else {
|
||||
// set new fusion algortithm
|
||||
filterChain = newFilterChain;
|
||||
fusionAlgorithm = revoSettings.FusionAlgorithm;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// read updated sensor UAVObjects and set initial state
|
||||
states.updated = updatedSensors;
|
||||
updatedSensors = 0;
|
||||
|
||||
// fetch sensors, check values, and load into state struct
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
|
||||
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
|
||||
gyroRaw[0] = states.gyro[0];
|
||||
gyroRaw[1] = states.gyro[1];
|
||||
gyroRaw[2] = states.gyro[2];
|
||||
}
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
|
||||
|
||||
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
|
||||
|
||||
// at this point sensor state is stored in "states" with some rudimentary filtering applied
|
||||
|
||||
// apply all filters in the current filter chain
|
||||
current = filterChain;
|
||||
|
||||
// we are not done, re-dispatch self execution
|
||||
runState = RUNSTATE_FILTER;
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
|
||||
break;
|
||||
|
||||
case RUNSTATE_FILTER:
|
||||
|
||||
if (current != NULL) {
|
||||
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
|
||||
if (result > alarm) {
|
||||
alarm = result;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
// we are not done, re-dispatch self execution
|
||||
if (!current) {
|
||||
runState = RUNSTATE_SAVE;
|
||||
}
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
|
||||
break;
|
||||
|
||||
case RUNSTATE_SAVE:
|
||||
|
||||
// the final output of filters is saved in state variables
|
||||
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
|
||||
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
|
||||
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
|
||||
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
|
||||
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
|
||||
}
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
|
||||
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
|
||||
MagStateData s;
|
||||
|
||||
MagStateGet(&s);
|
||||
s.x = states.mag[0];
|
||||
s.y = states.mag[1];
|
||||
s.z = states.mag[2];
|
||||
switch (states.magStatus) {
|
||||
case MAGSTATUS_OK:
|
||||
s.Source = MAGSTATE_SOURCE_ONBOARD;
|
||||
// check if a new filter chain should be initialized
|
||||
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
|
||||
FlightStatusData fs;
|
||||
FlightStatusGet(&fs);
|
||||
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
|
||||
const filterPipeline *newFilterChain;
|
||||
switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
|
||||
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
|
||||
newFilterChain = cfQueue;
|
||||
break;
|
||||
case MAGSTATUS_AUX:
|
||||
s.Source = MAGSTATE_SOURCE_AUX;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
|
||||
newFilterChain = cfmiQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
|
||||
newFilterChain = cfmQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
|
||||
newFilterChain = ekf13iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
|
||||
newFilterChain = ekf13Queue;
|
||||
break;
|
||||
default:
|
||||
s.Source = MAGSTATE_SOURCE_INVALID;
|
||||
newFilterChain = NULL;
|
||||
}
|
||||
// initialize filters in chain
|
||||
current = newFilterChain;
|
||||
bool error = 0;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->init((stateFilter *)current->filter);
|
||||
if (result != 0) {
|
||||
error = 1;
|
||||
break;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return;
|
||||
} else {
|
||||
// set new fusion algortithm
|
||||
filterChain = newFilterChain;
|
||||
fusionAlgorithm = revoSettings.FusionAlgorithm;
|
||||
}
|
||||
MagStateSet(&s);
|
||||
}
|
||||
}
|
||||
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
|
||||
// attitude nees manual conversion from quaternion to euler
|
||||
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
|
||||
AttitudeStateData s;
|
||||
AttitudeStateGet(&s);
|
||||
s.q1 = states.attitude[0];
|
||||
s.q2 = states.attitude[1];
|
||||
s.q3 = states.attitude[2];
|
||||
s.q4 = states.attitude[3];
|
||||
Quaternion2RPY(&s.q1, &s.Roll);
|
||||
AttitudeStateSet(&s);
|
||||
// read updated sensor UAVObjects and set initial state
|
||||
states.updated = updatedSensors;
|
||||
updatedSensors = 0;
|
||||
|
||||
// fetch sensors, check values, and load into state struct
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
|
||||
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
|
||||
gyroRaw[0] = states.gyro[0];
|
||||
gyroRaw[1] = states.gyro[1];
|
||||
gyroRaw[2] = states.gyro[2];
|
||||
}
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
|
||||
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
|
||||
|
||||
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
|
||||
|
||||
// at this point sensor state is stored in "states" with some rudimentary filtering applied
|
||||
|
||||
// apply all filters in the current filter chain
|
||||
current = filterChain;
|
||||
|
||||
// we are not done, re-dispatch self execution
|
||||
|
||||
while (current) {
|
||||
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
|
||||
if (result > alarm) {
|
||||
alarm = result;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
// throttle alarms, raise alarm flags immediately
|
||||
// but require system to run for a while before decreasing
|
||||
// to prevent alarm flapping
|
||||
if (alarm >= lastAlarm) {
|
||||
// the final output of filters is saved in state variables
|
||||
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
|
||||
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
|
||||
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
|
||||
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
|
||||
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
|
||||
}
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
|
||||
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
|
||||
MagStateData s;
|
||||
|
||||
MagStateGet(&s);
|
||||
s.x = states.mag[0];
|
||||
s.y = states.mag[1];
|
||||
s.z = states.mag[2];
|
||||
switch (states.magStatus) {
|
||||
case MAGSTATUS_OK:
|
||||
s.Source = MAGSTATE_SOURCE_ONBOARD;
|
||||
break;
|
||||
case MAGSTATUS_AUX:
|
||||
s.Source = MAGSTATE_SOURCE_AUX;
|
||||
break;
|
||||
default:
|
||||
s.Source = MAGSTATE_SOURCE_INVALID;
|
||||
}
|
||||
MagStateSet(&s);
|
||||
}
|
||||
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
|
||||
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
|
||||
// attitude nees manual conversion from quaternion to euler
|
||||
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
|
||||
AttitudeStateData s;
|
||||
AttitudeStateGet(&s);
|
||||
s.q1 = states.attitude[0];
|
||||
s.q2 = states.attitude[1];
|
||||
s.q3 = states.attitude[2];
|
||||
s.q4 = states.attitude[3];
|
||||
Quaternion2RPY(&s.q1, &s.Roll);
|
||||
AttitudeStateSet(&s);
|
||||
}
|
||||
// throttle alarms, raise alarm flags immediately
|
||||
// but require system to run for a while before decreasing
|
||||
// to prevent alarm flapping
|
||||
if (alarm >= lastAlarm) {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
} else {
|
||||
if (alarmcounter < 100) {
|
||||
alarmcounter++;
|
||||
} else {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
} else {
|
||||
if (alarmcounter < 100) {
|
||||
alarmcounter++;
|
||||
} else {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// clear alarms if everything is alright, then schedule callback execution after timeout
|
||||
if (lastAlarm == FILTERRESULT_WARNING) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else if (lastAlarm == FILTERRESULT_CRITICAL) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else if (lastAlarm >= FILTERRESULT_ERROR) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
// clear alarms if everything is alright, then schedule callback execution after timeout
|
||||
if (lastAlarm == FILTERRESULT_WARNING) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else if (lastAlarm == FILTERRESULT_CRITICAL) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else if (lastAlarm >= FILTERRESULT_ERROR) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
|
||||
// we are done, re-schedule next self execution
|
||||
runState = RUNSTATE_LOAD;
|
||||
if (updatedSensors) {
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
|
||||
} else {
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
break;
|
||||
if (updatedSensors) {
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
|
||||
} else {
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -130,8 +130,10 @@ int32_t TelemetryStart(void)
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
|
||||
|
||||
#ifdef PIOS_INCLUDE_RFM22B
|
||||
xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
|
||||
if (radioPort) {
|
||||
xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
|
||||
}
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
@ -432,14 +434,12 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
if (inputPort) {
|
||||
// Block until data are available
|
||||
uint8_t serial_data[1];
|
||||
uint8_t serial_data[16];
|
||||
uint16_t bytes_to_process;
|
||||
|
||||
bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
|
||||
if (bytes_to_process > 0) {
|
||||
for (uint8_t i = 0; i < bytes_to_process; i++) {
|
||||
UAVTalkProcessInputStream(uavTalkCon, serial_data[i]);
|
||||
}
|
||||
UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process);
|
||||
}
|
||||
} else {
|
||||
vTaskDelay(5);
|
||||
@ -457,14 +457,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
|
||||
while (1) {
|
||||
if (radioPort) {
|
||||
// Block until data are available
|
||||
uint8_t serial_data[1];
|
||||
uint8_t serial_data[16];
|
||||
uint16_t bytes_to_process;
|
||||
|
||||
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
|
||||
if (bytes_to_process > 0) {
|
||||
for (uint8_t i = 0; i < bytes_to_process; i++) {
|
||||
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
|
||||
}
|
||||
UAVTalkProcessInputStream(radioUavTalkCon, serial_data, bytes_to_process);
|
||||
}
|
||||
} else {
|
||||
vTaskDelay(5);
|
||||
|
||||
@ -57,8 +57,8 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
|
||||
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
|
||||
int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
|
||||
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
|
||||
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
|
||||
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
|
||||
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length);
|
||||
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
|
||||
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
|
||||
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);
|
||||
|
||||
@ -53,7 +53,15 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
|
||||
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
|
||||
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
|
||||
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
|
||||
|
||||
// UavTalk Process FSM functions
|
||||
static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_OBJID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_TIMESTAMP(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
|
||||
/**
|
||||
* Initialize the UAVTalk library
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
@ -350,10 +358,12 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
|
||||
/**
|
||||
* Process an byte from the telemetry stream.
|
||||
* \param[in] connectionHandle UAVTalkConnection to be used
|
||||
* \param[in] rxbyte Received byte
|
||||
* \param[in] rxbuffer Received buffer
|
||||
* \param[in/out] Length in bytes of received buffer
|
||||
* \param[in/out] position Next item to be read inside rxbuffer
|
||||
* \return UAVTalkRxState
|
||||
*/
|
||||
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
|
||||
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
|
||||
@ -361,231 +371,83 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
|
||||
|
||||
UAVTalkInputProcessor *iproc = &connection->iproc;
|
||||
|
||||
++connection->stats.rxBytes;
|
||||
|
||||
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
|
||||
iproc->state = UAVTALK_STATE_SYNC;
|
||||
}
|
||||
|
||||
if (iproc->rxPacketLength < 0xffff) {
|
||||
// update packet byte count
|
||||
iproc->rxPacketLength++;
|
||||
}
|
||||
uint8_t processedBytes = (*position);
|
||||
uint8_t count = 0;
|
||||
|
||||
// Receive state machine
|
||||
switch (iproc->state) {
|
||||
case UAVTALK_STATE_SYNC:
|
||||
|
||||
if (rxbyte != UAVTALK_SYNC_VAL) {
|
||||
connection->stats.rxSyncErrors++;
|
||||
// stop processing as soon as a complete packet is received, error is encountered or buffer is processed entirely
|
||||
while ((count = length - (*position)) > 0
|
||||
&& iproc->state != UAVTALK_STATE_COMPLETE
|
||||
&& iproc->state != UAVTALK_STATE_ERROR) {
|
||||
// Receive state machine
|
||||
if (iproc->state == UAVTALK_STATE_SYNC &&
|
||||
!UAVTalkProcess_SYNC(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Initialize and update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
|
||||
|
||||
iproc->rxPacketLength = 1;
|
||||
iproc->rxCount = 0;
|
||||
|
||||
iproc->type = 0;
|
||||
iproc->state = UAVTALK_STATE_TYPE;
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_TYPE:
|
||||
|
||||
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_SYNC;
|
||||
if (iproc->state == UAVTALK_STATE_TYPE &&
|
||||
!UAVTalkProcess_TYPE(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
iproc->type = rxbyte;
|
||||
|
||||
iproc->packet_size = 0;
|
||||
iproc->state = UAVTALK_STATE_SIZE;
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_SIZE:
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
if (iproc->rxCount == 0) {
|
||||
iproc->packet_size += rxbyte;
|
||||
iproc->rxCount++;
|
||||
break;
|
||||
}
|
||||
iproc->packet_size += rxbyte << 8;
|
||||
iproc->rxCount = 0;
|
||||
|
||||
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
|
||||
// incorrect packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
if (iproc->state == UAVTALK_STATE_SIZE &&
|
||||
!UAVTalkProcess_SIZE(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
iproc->objId = 0;
|
||||
iproc->state = UAVTALK_STATE_OBJID;
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_OBJID:
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
|
||||
if (iproc->rxCount < 4) {
|
||||
break;
|
||||
}
|
||||
iproc->rxCount = 0;
|
||||
|
||||
iproc->instId = 0;
|
||||
iproc->state = UAVTALK_STATE_INSTID;
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_INSTID:
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
|
||||
if (iproc->rxCount < 2) {
|
||||
break;
|
||||
}
|
||||
iproc->rxCount = 0;
|
||||
|
||||
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
|
||||
|
||||
// Determine data length
|
||||
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
|
||||
iproc->length = 0;
|
||||
iproc->timestampLength = 0;
|
||||
} else {
|
||||
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
|
||||
if (obj) {
|
||||
iproc->length = UAVObjGetNumBytes(obj);
|
||||
} else {
|
||||
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
|
||||
}
|
||||
}
|
||||
|
||||
// Check length
|
||||
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
|
||||
// packet error - exceeded payload max length
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
if (iproc->state == UAVTALK_STATE_OBJID &&
|
||||
!UAVTalkProcess_OBJID(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Check the lengths match
|
||||
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
|
||||
// packet error - mismatched packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
if (iproc->state == UAVTALK_STATE_INSTID &&
|
||||
!UAVTalkProcess_INSTID(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Determine next state
|
||||
if (iproc->type & UAVTALK_TIMESTAMPED) {
|
||||
// If there is a timestamp get it
|
||||
iproc->timestamp = 0;
|
||||
iproc->state = UAVTALK_STATE_TIMESTAMP;
|
||||
} else {
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (iproc->length > 0) {
|
||||
iproc->state = UAVTALK_STATE_DATA;
|
||||
} else {
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_TIMESTAMP:
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
|
||||
if (iproc->rxCount < 2) {
|
||||
break;
|
||||
}
|
||||
iproc->rxCount = 0;
|
||||
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (iproc->length > 0) {
|
||||
iproc->state = UAVTALK_STATE_DATA;
|
||||
} else {
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
}
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_DATA:
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
connection->rxBuffer[iproc->rxCount++] = rxbyte;
|
||||
if (iproc->rxCount < iproc->length) {
|
||||
break;
|
||||
}
|
||||
iproc->rxCount = 0;
|
||||
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
break;
|
||||
|
||||
case UAVTALK_STATE_CS:
|
||||
|
||||
// Check the CRC byte
|
||||
if (rxbyte != iproc->cs) {
|
||||
// packet error - faulty CRC
|
||||
UAVT_DEBUGLOG_PRINTF("BAD CRC");
|
||||
connection->stats.rxCrcErrors++;
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
if (iproc->state == UAVTALK_STATE_TIMESTAMP &&
|
||||
!UAVTalkProcess_TIMESTAMP(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
|
||||
// packet error - mismatched packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
if (iproc->state == UAVTALK_STATE_DATA &&
|
||||
!UAVTalkProcess_DATA(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
connection->stats.rxObjects++;
|
||||
connection->stats.rxObjectBytes += iproc->length;
|
||||
|
||||
iproc->state = UAVTALK_STATE_COMPLETE;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
break;
|
||||
if (iproc->state == UAVTALK_STATE_CS &&
|
||||
!UAVTalkProcess_CS(connection, iproc, rxbuffer, length, position)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
processedBytes = (*position) - processedBytes;
|
||||
connection->stats.rxBytes += processedBytes;
|
||||
return iproc->state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Process an byte from the telemetry stream.
|
||||
* Process a buffer from the telemetry stream.
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
* \param[in] rxbyte Received byte
|
||||
* \param[in] rxbuffer Received buffer
|
||||
* \param[in] count bytes inside rxbuffer
|
||||
* \return UAVTalkRxState
|
||||
*/
|
||||
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
|
||||
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length)
|
||||
{
|
||||
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
|
||||
uint8_t position = 0;
|
||||
UAVTalkRxState state = UAVTALK_STATE_ERROR;
|
||||
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
UAVTalkReceiveObject(connectionHandle);
|
||||
while (position < length) {
|
||||
state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbuffer, length, &position);
|
||||
if (state == UAVTALK_STATE_COMPLETE) {
|
||||
UAVTalkReceiveObject(connectionHandle);
|
||||
}
|
||||
}
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
@ -1000,6 +862,236 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Functions that implements the UAVTalk Process FSM. return false to break out of current cycle
|
||||
*/
|
||||
|
||||
static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
|
||||
{
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
|
||||
if (rxbyte != UAVTALK_SYNC_VAL) {
|
||||
connection->stats.rxSyncErrors++;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Initialize and update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
|
||||
|
||||
iproc->rxPacketLength = 1;
|
||||
iproc->rxCount = 0;
|
||||
|
||||
iproc->type = 0;
|
||||
iproc->state = UAVTALK_STATE_TYPE;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
|
||||
{
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
|
||||
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_SYNC;
|
||||
return false;
|
||||
}
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
iproc->type = rxbyte;
|
||||
iproc->rxPacketLength++;
|
||||
iproc->packet_size = 0;
|
||||
iproc->state = UAVTALK_STATE_SIZE;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
while (iproc->rxCount < 2 && length > (*position)) {
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
iproc->packet_size += rxbyte << 8 * iproc->rxCount;
|
||||
iproc->rxCount++;
|
||||
}
|
||||
|
||||
if (iproc->rxCount < 2) {
|
||||
return false;;
|
||||
}
|
||||
|
||||
iproc->rxCount = 0;
|
||||
|
||||
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
|
||||
// incorrect packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
return false;
|
||||
}
|
||||
iproc->rxPacketLength += 2;
|
||||
iproc->objId = 0;
|
||||
iproc->state = UAVTALK_STATE_OBJID;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_OBJID(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
while (iproc->rxCount < 4 && length > (*position)) {
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
|
||||
}
|
||||
|
||||
if (iproc->rxCount < 4) {
|
||||
return false;
|
||||
}
|
||||
iproc->rxCount = 0;
|
||||
iproc->rxPacketLength += 4;
|
||||
iproc->instId = 0;
|
||||
iproc->state = UAVTALK_STATE_INSTID;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
while (iproc->rxCount < 2 && length > (*position)) {
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
|
||||
}
|
||||
|
||||
if (iproc->rxCount < 2) {
|
||||
return false;
|
||||
}
|
||||
iproc->rxPacketLength += 2;
|
||||
iproc->rxCount = 0;
|
||||
|
||||
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
|
||||
|
||||
// Determine data length
|
||||
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
|
||||
iproc->length = 0;
|
||||
iproc->timestampLength = 0;
|
||||
} else {
|
||||
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
|
||||
if (obj) {
|
||||
iproc->length = UAVObjGetNumBytes(obj);
|
||||
} else {
|
||||
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
|
||||
}
|
||||
}
|
||||
|
||||
// Check length
|
||||
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
|
||||
// packet error - exceeded payload max length
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check the lengths match
|
||||
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
|
||||
// packet error - mismatched packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Determine next state
|
||||
if (iproc->type & UAVTALK_TIMESTAMPED) {
|
||||
// If there is a timestamp get it
|
||||
iproc->timestamp = 0;
|
||||
iproc->state = UAVTALK_STATE_TIMESTAMP;
|
||||
} else {
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (iproc->length > 0) {
|
||||
iproc->state = UAVTALK_STATE_DATA;
|
||||
} else {
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_TIMESTAMP(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
while (iproc->rxCount < 2 && length > (*position)) {
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
|
||||
}
|
||||
|
||||
if (iproc->rxCount < 2) {
|
||||
return false;;
|
||||
}
|
||||
|
||||
iproc->rxCount = 0;
|
||||
iproc->rxPacketLength += 2;
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (iproc->length > 0) {
|
||||
iproc->state = UAVTALK_STATE_DATA;
|
||||
} else {
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
|
||||
{
|
||||
uint8_t toCopy = iproc->length - iproc->rxCount;
|
||||
|
||||
if (toCopy > length - (*position)) {
|
||||
toCopy = length - (*position);
|
||||
}
|
||||
|
||||
memcpy(&connection->rxBuffer[iproc->rxCount], &rxbuffer[(*position)], toCopy);
|
||||
(*position) += toCopy;
|
||||
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateCRC(iproc->cs, &connection->rxBuffer[iproc->rxCount], toCopy);
|
||||
iproc->rxCount += toCopy;
|
||||
|
||||
iproc->rxPacketLength += toCopy;
|
||||
|
||||
if (iproc->rxCount < iproc->length) {
|
||||
return false;
|
||||
}
|
||||
|
||||
iproc->rxCount = 0;
|
||||
iproc->state = UAVTALK_STATE_CS;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
|
||||
{
|
||||
// Check the CRC byte
|
||||
uint8_t rxbyte = rxbuffer[(*position)++];
|
||||
|
||||
if (rxbyte != iproc->cs) {
|
||||
// packet error - faulty CRC
|
||||
UAVT_DEBUGLOG_PRINTF("BAD CRC");
|
||||
connection->stats.rxCrcErrors++;
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
return false;;
|
||||
}
|
||||
iproc->rxPacketLength++;
|
||||
|
||||
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
|
||||
// packet error - mismatched packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
return false;;
|
||||
}
|
||||
|
||||
connection->stats.rxObjects++;
|
||||
connection->stats.rxObjectBytes += iproc->length;
|
||||
|
||||
iproc->state = UAVTALK_STATE_COMPLETE;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user