rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

AHRS SPI comms: Retries were not being tracked correctly

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1863 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
les 2010-10-03 20:36:20 +00:00 committed by les
parent e743678d89
commit dcef777ab0
1 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/Libraries/ahrs_spi_comm.c
View File

@ -27,7 +27,7 @@ typedef enum { COMMS_NULL, COMMS_OBJECT, COMMS_PROGRAM } COMMSCOMMAND;
//The maximum number of objects that can be updated in one cycle.
//Currently the link is capable of sending 3 packets per cycle but 2 is enough
#define MAX_UPDATE_OBJECTS 3
#define MAX_UPDATE_OBJECTS 2
//Number of transmissions + 1 before we expect to see the data acknowledge
//This is controlled by the SPI hardware.
@ -65,14 +65,14 @@ static void SendPacket(void);
static void AhrsUpdatedCb(AhrsObjHandle handle);
#endif
/** Transmit data buffer
*/
static CommsDataPacket txPacket;
/** Receive data buffer
*/
static CommsDataPacket rxPacket;
/** Transmit data buffer
*/
static CommsDataPacket txPacket;
/** Objects that have changed and so should be transmitted
*/
static unsigned int dirtyObjects[MAX_AHRS_OBJECTS];
@ -115,7 +115,7 @@ void AhrsInitComms(void)
#ifdef IN_AHRS
PIOS_SPI_Init();
#else
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8); //72MHz/16 = 4.5MHz
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8); //36MHz/16 = 2.25MHz
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
AhrsObjHandle hdl = AhrsFromIndex(ct);
if (hdl) {
@ -158,7 +158,6 @@ void SetObjectDirty(const int idx)
}
dirtyObjects[idx] = ACK_LATENCY;
}
/** Work out what data needs to be sent.
If an object was not sent it will be retried 4 frames later
*/
@ -177,8 +176,8 @@ void FillObjectPacket()
AhrsObjHandle hdl = AhrsFromIndex(idx);
if (hdl) {
memcpy(&txPacket.objects[ct].object, hdl->data, hdl->size);
}
break;
}
} else {
dirtyObjects[idx]--;
if (dirtyObjects[idx] == 0) { //timed out
@ -189,6 +188,15 @@ void FillObjectPacket()
}
}
}
for (; idx < MAX_AHRS_OBJECTS; idx++) {
if (dirtyObjects[idx] > 0 && dirtyObjects[idx] != ACK_LATENCY) {
dirtyObjects[idx]--;
if (dirtyObjects[idx] == 0) { //timed out
dirtyObjects[idx] = ACK_LATENCY;
txPacket.status.retries++;
}
}
}
}
/** Process a received packet
@ -277,6 +285,8 @@ void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
if (rxPacket.magicNumber != RXMAGIC) {
crc_ok = false;
}
rxPacket.magicNumber = 0;
if (crc_ok) {
if (!linkOk && okCount > 0) {