rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Attitude: Do not start attitude estimation until accel data appears. This

Browse Source 
fixes the NaN when AttitudeSettings not available.
This commit is contained in:
James Cotton 2011-08-13 18:44:19 -05:00
parent 69f8aac967
commit a2b76adc33
3 changed files with 48 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
flight/Modules/Attitude/attitude.c
View File

@ -76,7 +76,7 @@ static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue;
static void updateSensors(AttitudeRawData *);
static int8_t updateSensors(AttitudeRawData *);
static void updateAttitude(AttitudeRawData *);
static void settingsUpdatedCb(UAVObjEvent * objEv);
@ -163,6 +163,11 @@ static void AttitudeTask(void *parameters)
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Set critical error and wait until the accel is producing data
while(PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
@ -197,14 +202,24 @@ static void AttitudeTask(void *parameters)
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
if(updateSensors(&attitudeRaw) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Only update attitude when sensor data is good
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
static void updateSensors(AttitudeRawData * attitudeRaw)
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static int8_t updateSensors(AttitudeRawData * attitudeRaw)
{
struct pios_adxl345_data accel_data;
float gyro[4];
@ -212,9 +227,12 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Only wait the time for two nominal updates before setting an alarm
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
return -1;
}
// No accel data available
if(PIOS_ADXL345_FifoElements() == 0)
return -1;
// First sample is temperature
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
@ -270,6 +288,8 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
return 0;
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
@ -307,6 +327,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors

16
flight/PiOS/Common/pios_adxl345.c
View File

@ -96,6 +96,22 @@ void PIOS_ADXL345_Init()
PIOS_ADXL345_SetMeasure(1);
}
/**
* @brief Return number of entries in the fifo
*/
uint8_t PIOS_ADXL345_FifoElements()
{
uint8_t buf[2] = {0,0};
uint8_t rec[2] = {0,0};
buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status
PIOS_ADXL345_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
PIOS_ADXL345_ReleaseBus();
return rec[1] & 0x3f;
}
/**
* @brief Read a single set of values from the x y z channels
* @returns The number of samples remaining in the fifo

2
flight/PiOS/inc/pios_adxl345.h
View File

@ -16,6 +16,7 @@
#define ADXL_MULTI_BIT 0x40
#define ADXL_X0_ADDR 0x32
#define ADXL_FIFOSTATUS_ADDR 0x39
#define ADXL_RATE_ADDR 0x2C
#define ADXL_RATE_100 0x0A
@ -52,5 +53,6 @@ void PIOS_ADXL345_FifoDepth(uint8_t depth);
void PIOS_ADXL345_Attach(uint32_t spi_id);
void PIOS_ADXL345_Init();
uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data);
uint8_t PIOS_ADXL345_FifoElements();
#endif