diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index d014babb1..cb41469c8 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -33,25 +33,26 @@ #include #include #include +#include // 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; diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 798a4ed6e..a1146f75e 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -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; + } } } } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5c999f01e..3ed9dfe02 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -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); } } diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 142c0a2ad..86d565891 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -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); diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index f66eb4a00..e485882ef 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -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); diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 5d618e046..aeb580b13 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -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; +} + + /** * @} * @}