rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-140 AHRS: Explicilty deal with if it is initialized or not, use that to

Browse Source 
determine retransmitting calibration, home location and such.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1755 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-25 21:05:38 +00:00 committed by peabody124
parent 4536bc38db
commit a01783b2a8
10 changed files with 111 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/AHRS/ahrs.c
View File

@ -178,6 +178,9 @@ uint8_t calibration_pending = FALSE;
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 25;
//! Flag for OP to check if AHRS has been initialized yet
enum initialized_mode initialized = 0;
/**
* @}
*/
@ -915,6 +918,16 @@ void process_spi_request(void)
ekf_too_slow;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_INITIALIZED:
// process incoming data
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_INITIALIZED);
user_tx_v1.payload.user.v.rsp.initialized.initialized = initialized;
if(user_rx_v1.payload.user.v.req.initialized.initialized == AHRS_INITIALIZED)
initialized = AHRS_INITIALIZED;
else if(user_rx_v1.payload.user.v.req.initialized.initialized == AHRS_UNINITIALIZED)
initialized = AHRS_UNINITIALIZED;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
default:
break;
}

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

@ -168,17 +168,11 @@ static void ahrscommsTask(void* parameters)
// Main task loop
while (1) {
struct opahrs_msg_v1 rsp;
struct opahrs_msg_v1 req;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
// data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
AhrsStatusSet(&data);
/* Spin here until we're in sync */
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
vTaskDelay(100 / portTICK_RATE_MS);
@ -192,17 +186,46 @@ static void ahrscommsTask(void* parameters)
} else {
/* Comms error */
continue;
}
}
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
AhrsStatusGet(&data);
req.payload.user.v.req.initialized.initialized = AHRS_INIT_QUERY;
result = PIOS_OPAHRS_SetGetInitialized(&req,&rsp);
if((result != OPAHRS_RESULT_OK) || (rsp.payload.user.v.rsp.initialized.initialized == AHRS_UNINITIALIZED)) {
data.Initialized = AHRSSTATUS_INITIALIZED_FALSE;
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
} else {
data.Initialized = AHRSSTATUS_INITIALIZED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
}
AhrsStatusSet(&data);
/* We're in sync with the AHRS, spin here until an error occurs */
lastSysTime = xTaskGetTickCount();
while (1) {
AHRSSettingsData settings;
/* Update settings with latest value */
AHRSSettingsGet(&settings);
AhrsStatusGet(&data);
if((data.HomeSet == AHRSSTATUS_HOMESET_TRUE) && (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_TRUE) && (data.AlgorithmSet == AHRSSTATUS_ALGORITHMSET_TRUE)) {
req.payload.user.v.req.initialized.initialized = AHRS_INITIALIZED;
if((result = PIOS_OPAHRS_SetGetInitialized(&req,&rsp)) == OPAHRS_RESULT_OK) {
data.Initialized = AHRSSTATUS_INITIALIZED_TRUE;
AhrsStatusSet(&data);
PIOS_OPAHRS_SetGetInitialized(&req,&rsp);
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
} else {
break;
}
}
// Update home coordinate if it hasn't been updated
AhrsStatusGet(&data);

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

@ -41,7 +41,7 @@
#define AHRSSTATUS_H
// Object constants
#define AHRSSTATUS_OBJID 1789860144U
#define AHRSSTATUS_OBJID 4061809000U
#define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1
@ -77,6 +77,7 @@ typedef struct {
uint8_t RunningTimePerCyle;
uint8_t DroppedUpdates;
uint8_t CommErrors[5];
uint8_t Initialized;
uint8_t AlgorithmSet;
uint8_t CalibrationSet;
uint8_t HomeSet;
@ -96,6 +97,9 @@ typedef struct {
typedef enum { AHRSSTATUS_COMMERRORS_ALGORITHM=0, AHRSSTATUS_COMMERRORS_UPDATE=1, AHRSSTATUS_COMMERRORS_ATTITUDERAW=2, AHRSSTATUS_COMMERRORS_HOMELOCATION=3, AHRSSTATUS_COMMERRORS_CALIBRATION=4 } AhrsStatusCommErrorsElem;
/* Number of elements for field CommErrors */
#define AHRSSTATUS_COMMERRORS_NUMELEM 5
// Field Initialized information
/* Enumeration options for field Initialized */
typedef enum { AHRSSTATUS_INITIALIZED_FALSE=0, AHRSSTATUS_INITIALIZED_TRUE=1 } AhrsStatusInitializedOptions;
// Field AlgorithmSet information
/* Enumeration options for field AlgorithmSet */
typedef enum { AHRSSTATUS_ALGORITHMSET_FALSE=0, AHRSSTATUS_ALGORITHMSET_TRUE=1 } AhrsStatusAlgorithmSetOptions;

19
flight/PiOS/Common/pios_opahrs.c
View File

@ -316,6 +316,25 @@ enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, stru
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_CALIBRATION, rsp);
}
enum opahrs_result PIOS_OPAHRS_SetGetInitialized(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp)
{
enum opahrs_result rc;
if (!req) return OPAHRS_RESULT_FAILED;
/* Make up an attituderaw request */
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_INITIALIZED);
/* Send the message until it is received */
rc = opahrs_msg_v1_send_req (req);
if (rc != OPAHRS_RESULT_OK) {
/* Failed to send the request, bail out */
return rc;
}
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_INITIALIZED, rsp);
}
#endif /* PIOS_INCLUDE_OPAHRS */
/**

1
flight/PiOS/inc/pios_opahrs.h
View File

@ -64,6 +64,7 @@ extern enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_SetAlgorithm(struct opahrs_msg_v1 *req);
extern enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req);
extern enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_SetGetInitialized(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_resync(void);
#endif /* PIOS_OPAHRS_H */

16
flight/PiOS/inc/pios_opahrs_proto.h
View File

@ -107,7 +107,7 @@ union opahrs_msg_v0_req {
struct opahrs_msg_v0_req_boot boot;
struct opahrs_msg_v0_req_serial serial;
struct opahrs_msg_v0_req_fwup_status status_req;
struct opahrs_msg_v0_req_mem_map mem_map;
struct opahrs_msg_v0_req_mem_map mem_map;
struct opahrs_msg_v0_req_fwup_start fwup_start;
struct opahrs_msg_v0_req_fwup_data fwup_data;
struct opahrs_msg_v0_req_fwup_verify fwup_verify;
@ -201,6 +201,12 @@ struct opahrs_msg_v1_req_reset {
struct opahrs_msg_v1_req_serial {
} __attribute__((__packed__));
enum initialized_mode {AHRS_UNINITIALIZED, AHRS_INITIALIZED, AHRS_INIT_QUERY};
struct opahrs_msg_v1_req_initialized {
enum initialized_mode initialized;
} __attribute__((__packed__));
struct opahrs_msg_v1_req_north {
float Be[3];
} __attribute__((__packed__));
@ -247,6 +253,7 @@ union opahrs_msg_v1_req {
struct opahrs_msg_v1_req_nop nop;
struct opahrs_msg_v1_req_versions versions;
struct opahrs_msg_v1_req_reset reset;
struct opahrs_msg_v1_req_initialized initialized;
struct opahrs_msg_v1_req_serial serial;
struct opahrs_msg_v1_req_update update;
struct opahrs_msg_v1_req_algorithm algorithm;
@ -265,6 +272,10 @@ struct opahrs_msg_v1_rsp_serial {
uint8_t serial_bcd[25];
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_initialized {
enum initialized_mode initialized;
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_north {
} __attribute__((__packed__));
@ -325,6 +336,7 @@ struct opahrs_msg_v1_rsp_calibration {
union opahrs_msg_v1_rsp {
struct opahrs_msg_v1_rsp_versions versions;
struct opahrs_msg_v1_rsp_serial serial;
struct opahrs_msg_v1_rsp_initialized initialized;
struct opahrs_msg_v1_rsp_north north;
struct opahrs_msg_v1_rsp_algorithm algorithm;
struct opahrs_msg_v1_rsp_attituderaw attituderaw;
@ -337,6 +349,7 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_REQ_VERSIONS,
OPAHRS_MSG_V1_REQ_RESET,
OPAHRS_MSG_V1_REQ_SERIAL,
OPAHRS_MSG_V1_REQ_INITIALIZED,
OPAHRS_MSG_V1_REQ_NORTH,
OPAHRS_MSG_V1_REQ_ALGORITHM,
OPAHRS_MSG_V1_REQ_UPDATE,
@ -345,6 +358,7 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_RSP_VERSIONS,
OPAHRS_MSG_V1_RSP_SERIAL,
OPAHRS_MSG_V1_RSP_INITIALIZED,
OPAHRS_MSG_V1_RSP_NORTH,
OPAHRS_MSG_V1_RSP_ALGORITHM,
OPAHRS_MSG_V1_RSP_UPDATE,

6
ground/src/plugins/uavobjects/ahrsstatus.cpp
View File

@ -88,6 +88,12 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
CommErrorsElemNames.append("HomeLocation");
CommErrorsElemNames.append("Calibration");
fields.append( new UAVObjectField(QString("CommErrors"), QString("count"), UAVObjectField::UINT8, CommErrorsElemNames, QStringList()) );
QStringList InitializedElemNames;
InitializedElemNames.append("0");
QStringList InitializedEnumOptions;
InitializedEnumOptions.append("FALSE");
InitializedEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("Initialized"), QString(""), UAVObjectField::ENUM, InitializedElemNames, InitializedEnumOptions) );
QStringList AlgorithmSetElemNames;
AlgorithmSetElemNames.append("0");
QStringList AlgorithmSetEnumOptions;

6
ground/src/plugins/uavobjects/ahrsstatus.h
View File

@ -49,6 +49,7 @@ public:
quint8 RunningTimePerCyle;
quint8 DroppedUpdates;
quint8 CommErrors[5];
quint8 Initialized;
quint8 AlgorithmSet;
quint8 CalibrationSet;
quint8 HomeSet;
@ -68,6 +69,9 @@ public:
typedef enum { COMMERRORS_ALGORITHM=0, COMMERRORS_UPDATE=1, COMMERRORS_ATTITUDERAW=2, COMMERRORS_HOMELOCATION=3, COMMERRORS_CALIBRATION=4 } CommErrorsElem;
/* Number of elements for field CommErrors */
static const quint32 COMMERRORS_NUMELEM = 5;
// Field Initialized information
/* Enumeration options for field Initialized */
typedef enum { INITIALIZED_FALSE=0, INITIALIZED_TRUE=1 } InitializedOptions;
// Field AlgorithmSet information
/* Enumeration options for field AlgorithmSet */
typedef enum { ALGORITHMSET_FALSE=0, ALGORITHMSET_TRUE=1 } AlgorithmSetOptions;
@ -80,7 +84,7 @@ public:
// Constants
static const quint32 OBJID = 1789860144U;
static const quint32 OBJID = 4061809000U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0;

14
ground/src/plugins/uavobjects/ahrsstatus.py
View File

@ -125,6 +125,18 @@ _fields = [ \
{
}
),
uavobject.UAVObjectField(
'Initialized',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'AlgorithmSet',
'b',
@ -166,7 +178,7 @@ _fields = [ \
class AhrsStatus(uavobject.UAVObject):
## Object constants
OBJID = 1789860144
OBJID = 4061809000
NAME = "AhrsStatus"
METANAME = "AhrsStatusMeta"
ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/ahrsstatus.xml
View File

@ -7,6 +7,7 @@
<field name="RunningTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<field name="DroppedUpdates" units="count" type="uint8" elements="1"/>
<field name="CommErrors" units="count" type="uint8" elementnames="Algorithm,Update,AttitudeRaw,HomeLocation,Calibration"/>
<field name="Initialized" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="AlgorithmSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="HomeSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>