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

OP-124 Flight: Added fields to the AHRSStatus to track failures of each of the SPI message types.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1347 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-21 09:21:39 +00:00 committed by peabody124
parent bb79ba66fa
commit ba4dc14cea
2 changed files with 33 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -119,14 +119,14 @@ int32_t AHRSCommsInitialize(void)
return 0;
}
static uint16_t attitude_errors = 0, attituderaw_errors = 0, position_errors = 0, home_errors = 0, altitude_errors = 0;
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void* parameters)
{
enum opahrs_result result;
uint16_t attitude_errors = 0, attituderaw_errors = 0,
position_errors = 0, home_errors = 0, altitude_errors = 0;
// Main task loop
while (1) {
@ -212,6 +212,7 @@ static void ahrscommsTask(void* parameters)
AHRSKnowsHome = TRUE;
} else {
/* Comms error */
PIOS_LED_Off(LED2);
home_errors++;
break;
}
@ -231,6 +232,23 @@ static void load_magnetic_north(struct opahrs_msg_v1_req_north * mag_north)
mag_north->Be[0] = data.Be[0];
mag_north->Be[1] = data.Be[1];
mag_north->Be[2] = data.Be[2];
if(data.Be[0] == 0 && data.Be[1] == 0 && data.Be[2] == 0)
{
// in case nothing has been set go to default to prevent NaN in attitude solution
mag_north->Be[0] = 1;
mag_north->Be[1] = 0;
mag_north->Be[2] = 0;
}
else {
// normalize for unit length here
float len = sqrt(data.Be[0] * data.Be[0] + data.Be[1] * data.Be[1] + data.Be[2] * data.Be[2]);
mag_north->Be[0] = data.Be[0] / len;
mag_north->Be[1] = data.Be[1] / len;
mag_north->Be[2] = data.Be[2] / len;
}
}
static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude)
@ -314,6 +332,12 @@ static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
data.SerialNumber[i] = serial->serial_bcd[i];
}
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDE] = attitude_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDERAW] = attituderaw_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_POSITIONACTUAL] = position_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ALTITUDE] = altitude_errors;
AhrsStatusSet(&data);
}

8
flight/OpenPilot/UAVObjects/inc/ahrsstatus.h
View File

@ -33,7 +33,7 @@
#define AHRSSTATUS_H
// Object constants
#define AHRSSTATUS_OBJID 3344048156U
#define AHRSSTATUS_OBJID 2618706832U
#define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1
@ -58,6 +58,7 @@
// Object data
typedef struct {
uint8_t SerialNumber[25];
uint8_t CommErrors[5];
} __attribute__((packed)) AhrsStatusData;
@ -65,6 +66,11 @@ typedef struct {
// Field SerialNumber information
/* Number of elements for field SerialNumber */
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
// Field CommErrors information
/* Array element names for field CommErrors */
typedef enum { AHRSSTATUS_COMMERRORS_ATTITUDE=0, AHRSSTATUS_COMMERRORS_ATTITUDERAW=1, AHRSSTATUS_COMMERRORS_POSITIONACTUAL=2, AHRSSTATUS_COMMERRORS_HOMELOCATION=3, AHRSSTATUS_COMMERRORS_ALTITUDE=4 } AhrsStatusCommErrorsElem;
/* Number of elements for field CommErrors */
#define AHRSSTATUS_COMMERRORS_NUMELEM 5
// Generic interface functions