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

Flight/AHRS Communications: Switch to Les' very nice UAVObject communication

Browse Source 
scheme

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1835 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-02 02:17:18 +00:00 committed by peabody124
parent 62300c9682
commit ea3fb03c7f
24 changed files with 1233 additions and 4092 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/AHRS/Makefile
View File

@ -82,7 +82,10 @@ STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
OPDIR = ../OpenPilot
OPUAVOBJ = $(OPDIR)/UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPSYSINC = $(OPDIR)/System/inc
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
@ -91,8 +94,10 @@ CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
SRC = ahrs.c
SRC += pios_board.c
SRC += ahrs_adc.c
SRC += ahrs_fsm.c
SRC += ahrs_timer.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
SRC += $(FLIGHTLIB)/ahrs_spi_program_slave.c
SRC += insgps.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
@ -110,7 +115,6 @@ SRC += $(PIOSSTM32F10X)/pios_exti.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_hmc5843.c
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## CMSIS for STM32
@ -172,6 +176,7 @@ EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(AHRSINC)
EXTRAINCDIRS += $(OPUAVOBJINC)
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
@ -216,6 +221,7 @@ DEBUGF = dwarf-2
CDEFS = -DSTM32F10X_$(MODEL)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
CDEFS += -DIN_AHRS
ifeq ($(USE_BOOTLOADER), YES)
CDEFS += -DUSE_BOOTLOADER
endif

1780
flight/AHRS/ahrs.c
View File

@ -1,927 +1,853 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
#include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#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
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
volatile enum algorithms ahrs_algorithm;
/**
* @addtogroup AHRS_Structures Local Structres
* @{
*/
//! Contains the data from the mag sensor chip
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
struct {
float axis[3];
} scaled;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} mag_data;
//! Contains the data from the accelerometer
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} accel_data;
//! Contains the data from the gyro
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
struct {
uint16_t xy;
uint16_t z;
} temp;
} gyro_data;
//! Conains the current estimate of the attitude
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
} attitude_data;
//! Contains data from the altitude sensor
struct altitude_sensor {
float altitude;
bool updated;
} altitude_data;
//! Contains data from the GPS (via the SPI link)
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
} gps_data;
/**
* @}
*/
/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void reset_values();
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! Indicates the communications are requesting a calibration
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;
/**
* @}
*/
/**
* @brief AHRS Main function
*/
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config(adc_oversampling);
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
/* SPI link to master */
PIOS_SPI_Init();
lfsm_init();
reset_values();
ahrs_state = AHRS_IDLE;
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
INSGPSInit();
while(initialized != AHRS_INITIALIZED)
process_spi_request();
#ifdef DUMP_RAW
int previous_conversion;
while (1) {
int result;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
ADC_CONTINUOUS_CHANNELS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while (1) {
// Alive signal
if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
if (calibration_pending) {
calibrate_sensors();
calibration_pending = FALSE;
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.updated = 1;
}
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while (ahrs_state != AHRS_DATA_READY) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/******************** INS ALGORITHM **************************/
if (ahrs_algorithm == INSGPS_Algo) {
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -mag_data.scaled.axis[1];
mag[1] = -mag_data.scaled.axis[0];
mag[2] = -mag_data.scaled.axis[2];
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
process_spi_request(); // get message out quickly
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && gps_data.quality == 1) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_data.groundspeed *
cos(gps_data.heading * M_PI / 180);
vel[1] =
gps_data.groundspeed *
sin(gps_data.heading * M_PI / 180);
INSSetPosVelVar(0.004);
if (gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED,
vel,
altitude_data.
altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,
vel,
altitude_data.
altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
} else if (gps_data.quality != -1
&& mag_data.updated == 1) {
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
if(mag_data.updated == 1) {
float mag_var[3] = {10,10,10};
INSSetMagVar(mag_var);
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
}
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
} else if (ahrs_algorithm == SIMPLE_Algo) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2((mag_data.raw.axis[0]),
(-1 * mag_data.raw.axis[1])) * 180 /
M_PI;
rpy[1] =
atan2(accel_data.filtered.x,
accel_data.filtered.z) * 180 / M_PI;
rpy[0] =
atan2(accel_data.filtered.y,
accel_data.filtered.z) * 180 / M_PI;
RPY2Quaternion(rpy, q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
(int16_t) (accel_data.filtered.x * 1000),
(int16_t) (accel_data.filtered.y * 1000),
(int16_t) (accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
(int16_t) (gyro_data.filtered.x * 1000),
(int16_t) (gyro_data.filtered.y * 1000),
(int16_t) (gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
(int16_t) (Nav.q[0] * 1000),
(int16_t) (Nav.q[1] * 1000),
(int16_t) (Nav.q[2] * 1000),
(int16_t) (Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] =
{ 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0 };
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & mag_data,
sizeof(mag_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gps_data,
sizeof(gps_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & accel_data,
sizeof(accel_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gyro_data,
sizeof(gyro_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
sizeof(float) * NUMX * NUMX);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
sizeof(float) * NUMX * NUMV);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
sizeof(float) * NUMX * NUMX);
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
}
return 0;
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
uint16_t i;
// Get the Y data. Third byte in. Convert to m/s
accel_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
// Get the Z data. Third byte in. Convert to m/s
accel_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.x /= fir_coeffs[adc_oversampling];
gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.y /= fir_coeffs[adc_oversampling];
gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.z /= fir_coeffs[adc_oversampling];
gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*
* After this function the bias for each sensor will be the mean value. This doesn't make
* sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read.
*/
#define NBIAS 100
#define NVAR 500
void calibrate_sensors()
{
int i,j;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
for (i = 0, j = 0; i < NBIAS; i++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
}
#endif
process_spi_request();
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
process_spi_request();
}
mag_data.calibration.variance[0] /= j;
mag_data.calibration.variance[1] /= j;
mag_data.calibration.variance[2] /= j;
}
/**
* @brief Populate fields with initial values
*/
void reset_values() {
accel_data.calibration.scale[0] = 0.012;
accel_data.calibration.scale[1] = 0.012;
accel_data.calibration.scale[2] = -0.012;
accel_data.calibration.bias[0] = 24;
accel_data.calibration.bias[1] = 24;
accel_data.calibration.bias[2] = -24;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 1;
mag_data.calibration.variance[1] = 1;
mag_data.calibration.variance[2] = 1;
}
/**
* @addtogroup AHRS_SPI SPI Messaging
* @{
* @brief SPI protocol handling requests for data from OP mainboard
*/
static struct opahrs_msg_v1 link_tx_v1;
static struct opahrs_msg_v1 link_rx_v1;
static struct opahrs_msg_v1 user_rx_v1;
static struct opahrs_msg_v1 user_tx_v1;
void process_spi_request(void)
{
bool msg_to_process = FALSE;
PIOS_IRQ_Disable();
/* Figure out if we're in an interesting stable state */
switch (lfsm_get_state()) {
case LFSM_STATE_USER_BUSY:
msg_to_process = TRUE;
break;
case LFSM_STATE_INACTIVE:
/* Queue up a receive buffer */
lfsm_user_set_rx_v1(&user_rx_v1);
lfsm_user_done();
break;
case LFSM_STATE_STOPPED:
/* Get things going */
lfsm_set_link_proto_v1(&link_tx_v1, &link_rx_v1);
break;
default:
/* Not a stable state */
break;
}
PIOS_IRQ_Enable();
if (!msg_to_process) {
/* Nothing to do */
return;
}
switch (user_rx_v1.payload.user.t) {
case OPAHRS_MSG_V1_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.
reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V1_REQ_SERIAL:
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_SERIAL);
PIOS_SYS_SerialNumberGet((char *) &(user_tx_v1.payload.user.v.rsp.
serial.serial_bcd));
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ALGORITHM:
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_ALGORITHM);
ahrs_algorithm = user_rx_v1.payload.user.v.req.algorithm.algorithm;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_NORTH:
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_NORTH);
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_CALIBRATION:
if (user_rx_v1.payload.user.v.req.calibration.
measure_var == AHRS_MEASURE) {
calibration_pending = TRUE;
} else if (user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_SET) {
// Set the accel calibration
accel_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0];
accel_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.accel_var[1];
accel_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
accel_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
accel_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
accel_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
accel_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
// Set the gyro calibration
gyro_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
gyro_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
gyro_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
gyro_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
gyro_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
gyro_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
gyro_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[0];
gyro_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[1];
gyro_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[2];
// Set the mag calibration
mag_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
mag_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
mag_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
mag_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.mag_scale[0];
mag_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.mag_scale[1];
mag_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.mag_scale[2];
mag_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
mag_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
mag_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
float zeros[3] = { 0, 0, 0 };
INSSetGyroBias(zeros); //gyro bias corrects in preprocessing
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
}
// echo back the values used
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_CALIBRATION);
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] = accel_data.calibration.variance[0];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] = accel_data.calibration.variance[1];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] = accel_data.calibration.variance[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_data.calibration.variance[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_data.calibration.variance[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_data.calibration.variance[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_data.calibration.variance[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_data.calibration.variance[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_data.calibration.variance[2];
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_ATTITUDERAW);
// Grab one sample from buffer to populate this
accel_data.raw.x = valid_data_buffer[0];
accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x =
mag_data.raw.axis[0];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y =
mag_data.raw.axis[1];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.z =
mag_data.raw.axis[2];
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x =
gyro_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y =
gyro_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z =
gyro_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
x = gyro_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
y = gyro_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
z = gyro_data.filtered.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp =
gyro_data.temp.xy;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp =
gyro_data.temp.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.x =
accel_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.y =
accel_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.z =
accel_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
x = accel_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
y = accel_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
z = accel_data.filtered.z;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_UPDATE:
// process incoming data
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_UPDATE);
if (user_rx_v1.payload.user.v.req.update.barometer.updated) {
altitude_data.altitude =
user_rx_v1.payload.user.v.req.update.barometer.
altitude;
altitude_data.updated =
user_rx_v1.payload.user.v.req.update.barometer.
updated;
}
if (user_rx_v1.payload.user.v.req.update.gps.updated) {
gps_data.updated = true;
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
gps_data.groundspeed = user_rx_v1.payload.user.v.req.update.gps.groundspeed;
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
}
// send out attitude/position estimate
user_tx_v1.payload.user.v.rsp.update.quaternion.q1 =
attitude_data.quaternion.q1;
user_tx_v1.payload.user.v.rsp.update.quaternion.q2 =
attitude_data.quaternion.q2;
user_tx_v1.payload.user.v.rsp.update.quaternion.q3 =
attitude_data.quaternion.q3;
user_tx_v1.payload.user.v.rsp.update.quaternion.q4 =
attitude_data.quaternion.q4;
// convert all to cm/s
user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0]*100;
user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1]*100;
user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2]*100;
user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0]*100;
user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1]*100;
user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2]*100;
// compute the idle fraction
user_tx_v1.payload.user.v.rsp.update.load =
((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
user_tx_v1.payload.user.v.rsp.update.idle_time =
idle_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.run_time =
running_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.dropped_updates =
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;
}
/* Finished processing the received message, requeue it */
lfsm_user_set_rx_v1(&user_rx_v1);
lfsm_user_done();
}
/**
* @}
*/
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
//#include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"
#include "ahrs_spi_comm.h"
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#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
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
volatile enum algorithms ahrs_algorithm;
/**
* @addtogroup AHRS_Structures Local Structres
* @{
*/
//! Contains the data from the mag sensor chip
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
struct {
float axis[3];
} scaled;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} mag_data;
//! Contains the data from the accelerometer
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} accel_data;
//! Contains the data from the gyro
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
struct {
uint16_t xy;
uint16_t z;
} temp;
} gyro_data;
//! Conains the current estimate of the attitude
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
} attitude_data;
//! Contains data from the altitude sensor
struct altitude_sensor {
float altitude;
bool updated;
} altitude_data;
//! Contains data from the GPS (via the SPI link)
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
} gps_data;
/**
* @}
*/
/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void reset_values();
void send_calibration(void);
void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
void gps_callback(AhrsObjHandle obj);
void settings_callback(AhrsObjHandle obj);
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 17;
/**
* @}
*/
/**
* @brief AHRS Main function
*/
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config(adc_oversampling);
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
/* SPI link to master */
// PIOS_SPI_Init();
// lfsm_init();
reset_values();
ahrs_state = AHRS_IDLE;
AhrsInitComms();
ahrs_state = AHRS_IDLE;
while(!AhrsLinkReady()) {
AhrsPoll();
while(ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
ahrs_state = AHRS_IDLE;
if((total_conversion_blocks % 10) == 0)
PIOS_LED_Toggle(LED1);
}
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
AHRSCalibrationConnectCallback(calibration_callback);
GPSPositionConnectCallback(gps_callback);
BaroAltitudeConnectCallback(altitude_callback);
AHRSSettingsConnectCallback(settings_callback);
calibration_callback(AHRSCalibrationHandle()); //force an update
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
INSGPSInit();
#ifdef DUMP_RAW
int previous_conversion;
while (1) {
AhrsPoll();
int result;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
ADC_CONTINUOUS_CHANNELS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = ((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
status.IdleTimePerCyle = idle_counts / (TIMER_RATE / 10000);
status.RunningTimePerCyle = running_counts / (TIMER_RATE / 10000);
status.DroppedUpdates = ekf_too_slow;
AhrsStatusSet(&status);
// Alive signal
if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.updated = 1;
}
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while (ahrs_state != AHRS_DATA_READY) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/******************** INS ALGORITHM **************************/
if (ahrs_algorithm == INSGPS_Algo) {
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -mag_data.scaled.axis[1];
mag[1] = -mag_data.scaled.axis[0];
mag[2] = -mag_data.scaled.axis[2];
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
process_spi_request(); // get message out quickly
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && gps_data.quality == 1) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_data.groundspeed *
cos(gps_data.heading * M_PI / 180);
vel[1] =
gps_data.groundspeed *
sin(gps_data.heading * M_PI / 180);
INSSetPosVelVar(0.004);
if (gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED,
vel,
altitude_data.
altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,
vel,
altitude_data.
altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
} else if (gps_data.quality != -1
&& mag_data.updated == 1) {
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
if(mag_data.updated == 1) {
float mag_var[3] = {10,10,10};
INSSetMagVar(mag_var);
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
}
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
} else if (ahrs_algorithm == SIMPLE_Algo) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2((mag_data.raw.axis[0]),
(-1 * mag_data.raw.axis[1])) * 180 /
M_PI;
rpy[1] =
atan2(accel_data.filtered.x,
accel_data.filtered.z) * 180 / M_PI;
rpy[0] =
atan2(accel_data.filtered.y,
accel_data.filtered.z) * 180 / M_PI;
RPY2Quaternion(rpy, q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
(int16_t) (accel_data.filtered.x * 1000),
(int16_t) (accel_data.filtered.y * 1000),
(int16_t) (accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
(int16_t) (gyro_data.filtered.x * 1000),
(int16_t) (gyro_data.filtered.y * 1000),
(int16_t) (gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
(int16_t) (Nav.q[0] * 1000),
(int16_t) (Nav.q[1] * 1000),
(int16_t) (Nav.q[2] * 1000),
(int16_t) (Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] =
{ 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0 };
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & mag_data,
sizeof(mag_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gps_data,
sizeof(gps_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & accel_data,
sizeof(accel_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gyro_data,
sizeof(gyro_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
sizeof(float) * NUMX * NUMX);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
sizeof(float) * NUMX * NUMV);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
sizeof(float) * NUMX * NUMX);
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
}
return 0;
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
uint16_t i;
// Get the Y data. Third byte in. Convert to m/s
accel_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
// Get the Z data. Third byte in. Convert to m/s
accel_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.x /= fir_coeffs[adc_oversampling];
gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.y /= fir_coeffs[adc_oversampling];
gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.z /= fir_coeffs[adc_oversampling];
gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
AttitudeRawData raw;
raw.gyros[0] = valid_data_buffer[1];
raw.gyros[1] = valid_data_buffer[3];
raw.gyros[2] = valid_data_buffer[5];
raw.gyros_filtered[0] = gyro_data.filtered.x;
raw.gyros_filtered[1] = gyro_data.filtered.y;
raw.gyros_filtered[2] = gyro_data.filtered.z;
raw.accels[0] = valid_data_buffer[2];
raw.accels[1] = valid_data_buffer[0];
raw.accels[2] = valid_data_buffer[4];
raw.accels_filtered[0] = accel_data.filtered.x;
raw.accels_filtered[1] = accel_data.filtered.y;
raw.accels_filtered[2] = accel_data.filtered.z;
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
AttitudeRawSet(&raw);
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*
* After this function the bias for each sensor will be the mean value. This doesn't make
* sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read.
*/
#define NBIAS 100
#define NVAR 500
void calibrate_sensors()
{
int i,j;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
for (i = 0, j = 0; i < NBIAS; i++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
}
#endif
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
}
mag_data.calibration.variance[0] /= j;
mag_data.calibration.variance[1] /= j;
mag_data.calibration.variance[2] /= j;
gyro_data.calibration.bias[0] -= gyro_bias[0];
gyro_data.calibration.bias[1] -= gyro_bias[1];
gyro_data.calibration.bias[2] -= gyro_bias[2];
}
/**
* @brief Populate fields with initial values
*/
void reset_values() {
accel_data.calibration.scale[0] = 0.012;
accel_data.calibration.scale[1] = 0.012;
accel_data.calibration.scale[2] = -0.012;
accel_data.calibration.bias[0] = 24;
accel_data.calibration.bias[1] = 24;
accel_data.calibration.bias[2] = -24;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 1;
mag_data.calibration.variance[1] = 1;
mag_data.calibration.variance[2] = 1;
}
void process_spi_request(void)
{
AttitudeActualData attitude;
attitude.q1 = attitude_data.quaternion.q1;
attitude.q2 = attitude_data.quaternion.q2;
attitude.q3 = attitude_data.quaternion.q3;
attitude.q4 = attitude_data.quaternion.q4;
float rpy[3];
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
attitude.Roll = rpy[0];
attitude.Pitch = rpy[1];
attitude.Yaw = rpy[2];
AttitudeActualSet(&attitude);
}
void send_calibration(void)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
for(int ct=0; ct<3; ct++)
{
cal.accel_var[ct] = accel_data.calibration.variance[ct];
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
cal.mag_var[ct] = mag_data.calibration.variance[ct];
}
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
/**
* @brief AHRS calibration callback
*
* Called when the OP board sets the calibration
*/
void calibration_callback(AhrsObjHandle obj)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct] = cal.accel_scale[ct];
accel_data.calibration.bias[ct] = cal.accel_bias[ct];
accel_data.calibration.variance[ct] = cal.accel_var[ct];
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
}
}else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE){
calibrate_sensors();
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_ECHO){
send_calibration();
}
}
void gps_callback(AhrsObjHandle obj)
{
GPSPositionData pos;
GPSPositionGet(&pos);
HomeLocationData home;
HomeLocationGet(&home);
if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
gps_data.NED[0] = 0;
gps_data.NED[1] = 0;
gps_data.NED[2] = 0;
gps_data.groundspeed = 0;
gps_data.heading = 0;
gps_data.quality = -1; // indicates indoor mode, high variance zeros update
gps_data.updated = true;
return;
}
if(pos.Status != GPSPOSITION_STATUS_FIX3D) //FIXME: Will this work? the old ahrs_comms does it differently.
{
gps_data.quality = 0;
gps_data.updated = true;
return;
}
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1;
gps_data.updated = true;
}
void altitude_callback(AhrsObjHandle obj)
{
BaroAltitudeData alt;
BaroAltitudeGet(&alt);
altitude_data.altitude = alt.Altitude;
altitude_data.updated = true;
}
void settings_callback(AhrsObjHandle obj)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
{
ahrs_algorithm = INSGPS_Algo;
}else
{
ahrs_algorithm = SIMPLE_Algo;
}
}
/**
* @}
*/

735
flight/AHRS/ahrs_fsm.c
View File

@ -1,735 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_ADC AHRS ADC
* @brief Specialized ADC code for double buffered DMA for AHRS
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <stdint.h> /* uint*_t */
#include <stddef.h> /* NULL */
#include "ahrs_fsm.h"
#include "pios_opahrs_proto.h"
#include "pios.h"
struct lfsm_context {
enum lfsm_state curr_state;
enum opahrs_msg_link_state link_state;
enum opahrs_msg_type user_payload_type;
uint32_t user_payload_len;
uint32_t errors;
uint8_t *rx;
uint8_t *tx;
uint8_t *link_rx;
uint8_t *link_tx;
uint8_t *user_rx;
uint8_t *user_tx;
struct lfsm_link_stats stats;
};
static struct lfsm_context context = { 0 };
static void lfsm_update_link_tx(struct lfsm_context *context);
static void lfsm_init_rx(struct lfsm_context *context);
/*
*
* Link Finite State Machine
*
*/
struct lfsm_transition {
void (*entry_fn) (struct lfsm_context * context);
enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
};
static void go_faulted(struct lfsm_context *context);
static void go_stopped(struct lfsm_context *context);
static void go_stopping(struct lfsm_context *context);
static void go_inactive(struct lfsm_context *context);
static void go_user_busy(struct lfsm_context *context);
static void go_user_busy_rx_pending(struct lfsm_context *context);
static void go_user_busy_tx_pending(struct lfsm_context *context);
static void go_user_busy_rxtx_pending(struct lfsm_context *context);
static void go_user_rx_pending(struct lfsm_context *context);
static void go_user_tx_pending(struct lfsm_context *context);
static void go_user_rxtx_pending(struct lfsm_context *context);
static void go_user_rx_active(struct lfsm_context *context);
static void go_user_tx_active(struct lfsm_context *context);
static void go_user_rxtx_active(struct lfsm_context *context);
const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES]
= {
[LFSM_STATE_FAULTED] = {
.entry_fn = go_faulted,
},
[LFSM_STATE_STOPPED] = {
.entry_fn = go_stopped,
.next_state = {
[LFSM_EVENT_INIT_LINK] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_STOPPING] = {
.entry_fn = go_stopping,
.next_state = {
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_INACTIVE] = {
.entry_fn = go_inactive,
.next_state = {
[LFSM_EVENT_STOP] =
LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] =
LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] =
LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_BUSY] = {
.entry_fn = go_user_busy,
.next_state = {
[LFSM_EVENT_STOP] =
LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] =
LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] =
LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_USER_DONE] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_USER_BUSY,
},
},
[LFSM_STATE_USER_BUSY_RX_PENDING] = {
.entry_fn =
go_user_busy_rx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_TX_PENDING] = {
.entry_fn =
go_user_busy_tx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
.entry_fn =
go_user_busy_rxtx_pending,
.next_state = {
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
},
},
[LFSM_STATE_USER_RX_PENDING] = {
.entry_fn = go_user_rx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_PENDING] = {
.entry_fn = go_user_tx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
},
},
[LFSM_STATE_USER_RXTX_PENDING] = {
.entry_fn = go_user_rxtx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
},
},
[LFSM_STATE_USER_RX_ACTIVE] = {
.entry_fn = go_user_rx_active,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_ACTIVE] = {
.entry_fn = go_user_tx_active,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_RXTX_ACTIVE] = {
.entry_fn = go_user_rxtx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
};
/*
* FSM State Entry Functions
*/
static void go_faulted(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(0);
}
static void go_stopped(struct lfsm_context *context)
{
#if 0
PIOS_SPI_Stop(PIOS_SPI_OP);
#endif
}
static void go_stopping(struct lfsm_context *context)
{
context->link_tx = NULL;
context->tx = NULL;
}
static void go_inactive(struct lfsm_context *context)
{
context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
lfsm_update_link_tx(context);
context->user_rx = NULL;
context->user_tx = NULL;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->user_rx = NULL;
context->user_tx = NULL;
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_rx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_tx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_rxtx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_tx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rxtx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->rx = context->user_rx;
context->tx = context->link_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
lfsm_update_link_tx(context);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_tx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
context->rx = context->link_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rxtx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
context->rx = context->user_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
/*
*
* Misc Helper Functions
*
*/
static void lfsm_update_link_tx_v0(struct opahrs_msg_v0 *msg,
enum opahrs_msg_link_state state,
uint16_t errors)
{
opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx_v1(struct opahrs_msg_v1 *msg,
enum opahrs_msg_link_state state,
uint16_t errors)
{
opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(context->link_tx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
lfsm_update_link_tx_v0((struct opahrs_msg_v0 *)context->
link_tx, context->link_state,
context->errors);
break;
case OPAHRS_MSG_TYPE_USER_V1:
lfsm_update_link_tx_v1((struct opahrs_msg_v1 *)context->
link_tx, context->link_state,
context->errors);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
static void lfsm_init_rx(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(context->rx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
opahrs_msg_v0_init_rx((struct opahrs_msg_v0 *)context->rx);
break;
case OPAHRS_MSG_TYPE_USER_V1:
opahrs_msg_v1_init_rx((struct opahrs_msg_v1 *)context->rx);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
/*
*
* External API
*
*/
void lfsm_inject_event(enum lfsm_event event)
{
PIOS_IRQ_Disable();
/*
* Move to the next state
*
* This is done prior to calling the new state's entry function to
* guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was.
*/
context.curr_state =
lfsm_transitions[context.curr_state].next_state[event];
/* Call the entry function (if any) for the next state. */
if (lfsm_transitions[context.curr_state].entry_fn) {
lfsm_transitions[context.curr_state].entry_fn(&context);
}
PIOS_IRQ_Enable();
}
void lfsm_init(void)
{
context.curr_state = LFSM_STATE_STOPPED;
go_stopped(&context);
}
void lfsm_set_link_proto_v0(struct opahrs_msg_v0 *link_tx,
struct opahrs_msg_v0 *link_rx)
{
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *) link_tx;
context.link_rx = (uint8_t *) link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v0(link_tx, context.link_state,
context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_set_link_proto_v1(struct opahrs_msg_v1 *link_tx,
struct opahrs_msg_v1 *link_rx)
{
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *) link_tx;
context.link_rx = (uint8_t *) link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v1(link_tx, context.link_state,
context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_user_set_tx_v0(struct opahrs_msg_v0 *user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V0);
context.user_tx = (uint8_t *) user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v0(struct opahrs_msg_v0 *user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V0);
context.user_rx = (uint8_t *) user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_set_tx_v1(struct opahrs_msg_v1 *user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V1);
context.user_tx = (uint8_t *) user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v1(struct opahrs_msg_v1 *user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V1);
context.user_rx = (uint8_t *) user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_done(void)
{
lfsm_inject_event(LFSM_EVENT_USER_DONE);
}
void lfsm_stop(void)
{
lfsm_inject_event(LFSM_EVENT_STOP);
}
void lfsm_get_link_stats(struct lfsm_link_stats *stats)
{
PIOS_DEBUG_Assert(stats);
*stats = context.stats;
}
enum lfsm_state lfsm_get_state(void)
{
return context.curr_state;
}
/*
*
* ISR Callback
*
*/
void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val)
{
if (!crc_ok) {
context.stats.rx_badcrc++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
if (!context.rx) {
/* No way to know what we just received, assume invalid */
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Recover the head and tail pointers from the message */
struct opahrs_msg_link_head *head = NULL;
struct opahrs_msg_link_tail *tail = NULL;
switch (context.user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
head = &((struct opahrs_msg_v0 *)context.rx)->head;
tail = &((struct opahrs_msg_v0 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_USER_V1:
head = &((struct opahrs_msg_v1 *)context.rx)->head;
tail = &((struct opahrs_msg_v1 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_LINK:
/* Should never be rx'ing before the link protocol version is known */
PIOS_DEBUG_Assert(0);
break;
}
/* Check for bad magic */
if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) ||
(tail->magic != OPAHRS_MSG_MAGIC_TAIL)) {
if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
context.stats.rx_badmagic_head++;
}
if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
context.stats.rx_badmagic_tail++;
}
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Good magic, find out what type of payload we've got */
switch (head->type) {
case OPAHRS_MSG_TYPE_LINK:
context.stats.rx_link++;
lfsm_inject_event(LFSM_EVENT_RX_LINK);
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
if (head->type == context.user_payload_type) {
context.stats.rx_user++;
lfsm_inject_event(LFSM_EVENT_RX_USER);
} else {
/* Mismatched user payload type */
context.stats.rx_badver++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
}
break;
default:
/* Unidentifiable payload type */
context.stats.rx_badtype++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
}
}
/**
* @}
* @}
*/

853
flight/AHRS/ahrs_newspi.c
View File

@ -1,853 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
//#include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"
#include "ahrs_spi_comm.h"
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#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
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
volatile enum algorithms ahrs_algorithm;
/**
* @addtogroup AHRS_Structures Local Structres
* @{
*/
//! Contains the data from the mag sensor chip
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
struct {
float axis[3];
} scaled;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} mag_data;
//! Contains the data from the accelerometer
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
} accel_data;
//! Contains the data from the gyro
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
struct {
uint16_t xy;
uint16_t z;
} temp;
} gyro_data;
//! Conains the current estimate of the attitude
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
} attitude_data;
//! Contains data from the altitude sensor
struct altitude_sensor {
float altitude;
bool updated;
} altitude_data;
//! Contains data from the GPS (via the SPI link)
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
} gps_data;
/**
* @}
*/
/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void reset_values();
void send_calibration(void);
void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
void gps_callback(AhrsObjHandle obj);
void settings_callback(AhrsObjHandle obj);
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 17;
/**
* @}
*/
/**
* @brief AHRS Main function
*/
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config(adc_oversampling);
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
/* SPI link to master */
// PIOS_SPI_Init();
// lfsm_init();
reset_values();
ahrs_state = AHRS_IDLE;
AhrsInitComms();
ahrs_state = AHRS_IDLE;
while(!AhrsLinkReady()) {
AhrsPoll();
while(ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
ahrs_state = AHRS_IDLE;
if((total_conversion_blocks % 10) == 0)
PIOS_LED_Toggle(LED1);
}
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
AHRSCalibrationConnectCallback(calibration_callback);
GPSPositionConnectCallback(gps_callback);
BaroAltitudeConnectCallback(altitude_callback);
AHRSSettingsConnectCallback(settings_callback);
calibration_callback(AHRSCalibrationHandle()); //force an update
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
INSGPSInit();
#ifdef DUMP_RAW
int previous_conversion;
while (1) {
AhrsPoll();
int result;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
ADC_CONTINUOUS_CHANNELS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatus2Data status;
AhrsStatus2Get(&status);
status.CPULoad = ((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
status.IdleTimePerCyle = idle_counts / (TIMER_RATE / 10000);
status.RunningTimePerCyle = running_counts / (TIMER_RATE / 10000);
status.DroppedUpdates = ekf_too_slow;
AhrsStatus2Set(&status);
// Alive signal
if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.updated = 1;
}
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while (ahrs_state != AHRS_DATA_READY) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/******************** INS ALGORITHM **************************/
if (ahrs_algorithm == INSGPS_Algo) {
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -mag_data.scaled.axis[1];
mag[1] = -mag_data.scaled.axis[0];
mag[2] = -mag_data.scaled.axis[2];
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
process_spi_request(); // get message out quickly
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && gps_data.quality == 1) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_data.groundspeed *
cos(gps_data.heading * M_PI / 180);
vel[1] =
gps_data.groundspeed *
sin(gps_data.heading * M_PI / 180);
INSSetPosVelVar(0.004);
if (gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED,
vel,
altitude_data.
altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,
vel,
altitude_data.
altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
} else if (gps_data.quality != -1
&& mag_data.updated == 1) {
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
if(mag_data.updated == 1) {
float mag_var[3] = {10,10,10};
INSSetMagVar(mag_var);
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
}
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
} else if (ahrs_algorithm == SIMPLE_Algo) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2((mag_data.raw.axis[0]),
(-1 * mag_data.raw.axis[1])) * 180 /
M_PI;
rpy[1] =
atan2(accel_data.filtered.x,
accel_data.filtered.z) * 180 / M_PI;
rpy[0] =
atan2(accel_data.filtered.y,
accel_data.filtered.z) * 180 / M_PI;
RPY2Quaternion(rpy, q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
(int16_t) (accel_data.filtered.x * 1000),
(int16_t) (accel_data.filtered.y * 1000),
(int16_t) (accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
(int16_t) (gyro_data.filtered.x * 1000),
(int16_t) (gyro_data.filtered.y * 1000),
(int16_t) (gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
(int16_t) (Nav.q[0] * 1000),
(int16_t) (Nav.q[1] * 1000),
(int16_t) (Nav.q[2] * 1000),
(int16_t) (Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] =
{ 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0 };
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & mag_data,
sizeof(mag_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gps_data,
sizeof(gps_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & accel_data,
sizeof(accel_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gyro_data,
sizeof(gyro_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
sizeof(float) * NUMX * NUMX);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
sizeof(float) * NUMX * NUMV);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
sizeof(float) * NUMX * NUMX);
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
}
return 0;
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
uint16_t i;
// Get the Y data. Third byte in. Convert to m/s
accel_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
// Get the Z data. Third byte in. Convert to m/s
accel_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling];
accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_data.filtered.x = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.x /= fir_coeffs[adc_oversampling];
gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_data.filtered.y = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.y /= fir_coeffs[adc_oversampling];
gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_data.filtered.z = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
gyro_data.filtered.z /= fir_coeffs[adc_oversampling];
gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
AttitudeRawData raw;
raw.gyros[0] = valid_data_buffer[1];
raw.gyros[1] = valid_data_buffer[3];
raw.gyros[2] = valid_data_buffer[5];
raw.gyros_filtered[0] = gyro_data.filtered.x;
raw.gyros_filtered[1] = gyro_data.filtered.y;
raw.gyros_filtered[2] = gyro_data.filtered.z;
raw.accels[0] = valid_data_buffer[2];
raw.accels[1] = valid_data_buffer[0];
raw.accels[2] = valid_data_buffer[4];
raw.accels_filtered[0] = accel_data.filtered.x;
raw.accels_filtered[1] = accel_data.filtered.y;
raw.accels_filtered[2] = accel_data.filtered.z;
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
AttitudeRawSet(&raw);
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*
* After this function the bias for each sensor will be the mean value. This doesn't make
* sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read.
*/
#define NBIAS 100
#define NVAR 500
void calibrate_sensors()
{
int i,j;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
for (i = 0, j = 0; i < NBIAS; i++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
}
#endif
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
ahrs_state = AHRS_IDLE;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
}
mag_data.calibration.variance[0] /= j;
mag_data.calibration.variance[1] /= j;
mag_data.calibration.variance[2] /= j;
gyro_data.calibration.bias[0] -= gyro_bias[0];
gyro_data.calibration.bias[1] -= gyro_bias[1];
gyro_data.calibration.bias[2] -= gyro_bias[2];
}
/**
* @brief Populate fields with initial values
*/
void reset_values() {
accel_data.calibration.scale[0] = 0.012;
accel_data.calibration.scale[1] = 0.012;
accel_data.calibration.scale[2] = -0.012;
accel_data.calibration.bias[0] = 24;
accel_data.calibration.bias[1] = 24;
accel_data.calibration.bias[2] = -24;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 1;
mag_data.calibration.variance[1] = 1;
mag_data.calibration.variance[2] = 1;
}
void process_spi_request(void)
{
AttitudeActualData attitude;
attitude.q1 = attitude_data.quaternion.q1;
attitude.q2 = attitude_data.quaternion.q2;
attitude.q3 = attitude_data.quaternion.q3;
attitude.q4 = attitude_data.quaternion.q4;
float rpy[3];
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
attitude.Roll = rpy[0];
attitude.Pitch = rpy[1];
attitude.Yaw = rpy[2];
AttitudeActualSet(&attitude);
}
void send_calibration(void)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
for(int ct=0; ct<3; ct++)
{
cal.accel_var[ct] = accel_data.calibration.variance[ct];
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
cal.mag_var[ct] = mag_data.calibration.variance[ct];
}
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
/**
* @brief AHRS calibration callback
*
* Called when the OP board sets the calibration
*/
void calibration_callback(AhrsObjHandle obj)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct] = cal.accel_scale[ct];
accel_data.calibration.bias[ct] = cal.accel_bias[ct];
accel_data.calibration.variance[ct] = cal.accel_var[ct];
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
}
}else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE){
calibrate_sensors();
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_ECHO){
send_calibration();
}
}
void gps_callback(AhrsObjHandle obj)
{
GPSPositionData pos;
GPSPositionGet(&pos);
HomeLocationData home;
HomeLocationGet(&home);
if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
gps_data.NED[0] = 0;
gps_data.NED[1] = 0;
gps_data.NED[2] = 0;
gps_data.groundspeed = 0;
gps_data.heading = 0;
gps_data.quality = -1; // indicates indoor mode, high variance zeros update
gps_data.updated = true;
return;
}
if(pos.Status != GPSPOSITION_STATUS_FIX3D) //FIXME: Will this work? the old ahrs_comms does it differently.
{
gps_data.quality = 0;
gps_data.updated = true;
return;
}
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1;
gps_data.updated = true;
}
void altitude_callback(AhrsObjHandle obj)
{
BaroAltitudeData alt;
BaroAltitudeGet(&alt);
altitude_data.altitude = alt.Altitude;
altitude_data.updated = true;
}
void settings_callback(AhrsObjHandle obj)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
{
ahrs_algorithm = INSGPS_Algo;
}else
{
ahrs_algorithm = SIMPLE_Algo;
}
}
/**
* @}
*/

6
flight/Libraries/ahrs_comm_objects.c
View File

@ -5,7 +5,7 @@ static AttitudeRawData AttitudeRaw;
static AttitudeActualData AttitudeActual;
static AHRSCalibrationData AHRSCalibration;
static AttitudeSettingsData AttitudeSettings;
static AhrsStatus2Data AhrsStatus2;
static AhrsStatusData AhrsStatus;
static BaroAltitudeData BaroAltitude;
static GPSPositionData GPSPosition;
static PositionActualData PositionActual;
@ -28,7 +28,7 @@ CREATEHANDLE( 0, AttitudeRaw );
CREATEHANDLE( 1, AttitudeActual );
CREATEHANDLE( 2, AHRSCalibration );
CREATEHANDLE( 3, AttitudeSettings );
CREATEHANDLE( 4, AhrsStatus2 );
CREATEHANDLE( 4, AhrsStatus );
CREATEHANDLE( 5, BaroAltitude );
CREATEHANDLE( 6, GPSPosition );
CREATEHANDLE( 7, PositionActual );
@ -70,7 +70,7 @@ void AhrsInitHandles( void )
ADDHANDLE( idx++, AttitudeActual );
ADDHANDLE( idx++, AHRSCalibration );
ADDHANDLE( idx++, AttitudeSettings );
ADDHANDLE( idx++, AhrsStatus2 );
ADDHANDLE( idx++, AhrsStatus );
ADDHANDLE( idx++, BaroAltitude );
ADDHANDLE( idx++, GPSPosition );
ADDHANDLE( idx++, PositionActual );

4
flight/Libraries/inc/ahrs_comm_objects.h
View File

@ -4,7 +4,7 @@
#include "attitudeactual.h"
#include "attituderaw.h"
#include "attitudesettings.h"
#include "ahrsstatus2.h"
#include "ahrsstatus.h"
#include "baroaltitude.h"
#include "gpsposition.h"
#include "positionactual.h"
@ -20,7 +20,7 @@ typedef union {
AttitudeActualData AttitudeActual;
AHRSCalibrationData AHRSCalibration;
AttitudeSettingsData AttitudeSettings;
AhrsStatus2Data AhrsStatus2;
AhrsStatusData AhrsStatus;
BaroAltitudeData BaroAltitude;
GPSPositionData GPSPosition;
PositionActualData PositionActual;

7
flight/OpenPilot/Makefile
View File

@ -217,10 +217,11 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_bmp085.c
SRC += $(PIOSCOMMON)/pios_opahrs.c
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
#SRC += $(PIOSCOMMON)/pios_opahrs.c
#SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c

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

@ -1,570 +1,142 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @brief Handles communication with AHRS and updating position
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file ahrs_comms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input object: AttitudeSettings
* Output object: AttitudeActual
*
* This module will periodically update the value of latest attitude solution
* that is available from the AHRS.
* The module settings can configure how often AHRS is polled for a new solution.
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "ahrs_comms.h"
#include "attitudeactual.h"
#include "ahrssettings.h"
#include "attituderaw.h"
#include "attitudesettings.h"
#include "ahrsstatus.h"
#include "alarms.h"
#include "baroaltitude.h"
#include "stdbool.h"
#include "gpsposition.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "CoordinateConversions.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"
// Private constants
#define STACK_SIZE 450
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
// Private types
// Private variables
static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void *parameters);
static void load_baro_altitude(struct opahrs_msg_v1_req_update *update);
static void load_gps_position(struct opahrs_msg_v1_req_update *update);
static void load_magnetic_north(struct opahrs_msg_v1_req_north *north);
static void load_calibration(struct opahrs_msg_v1_req_calibration *calibration);
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw *attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial *serial);
static void update_calibration(struct opahrs_msg_v1_rsp_calibration *calibration);
static void process_update(struct opahrs_msg_v1_rsp_update *update); // main information parser
static bool AHRSSettingsIsUpdatedFlag = false;
static void AHRSSettingsUpdatedCb(UAVObjEvent * ev)
{
AHRSSettingsIsUpdatedFlag = true;
}
static bool BaroAltitudeIsUpdatedFlag = false;
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
{
BaroAltitudeIsUpdatedFlag = true;
}
static bool GPSPositionIsUpdatedFlag = false;
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{
GPSPositionIsUpdatedFlag = true;
}
static bool HomeLocationIsUpdatedFlag = false;
static void HomeLocationUpdatedCb(UAVObjEvent * ev)
{
HomeLocationIsUpdatedFlag = true;
}
static bool AHRSCalibrationIsUpdatedFlag = false;
static bool AHRSCalibrationIsLocallyUpdateFlag = false;
static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
{
if (AHRSCalibrationIsLocallyUpdateFlag == true)
AHRSCalibrationIsLocallyUpdateFlag = false;
else
AHRSCalibrationIsUpdatedFlag = true;
}
static uint32_t GPSGoodUpdates;
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AHRSSettingsConnectCallback(AHRSSettingsUpdatedCb);
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
GPSPositionConnectCallback(GPSPositionUpdatedCb);
HomeLocationConnectCallback(HomeLocationUpdatedCb);
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
PIOS_OPAHRS_Init();
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
static uint16_t update_errors = 0;
static uint16_t attituderaw_errors = 0;
static uint16_t home_errors = 0;
static uint16_t calibration_errors = 0;
static uint16_t algorithm_errors = 0;
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void *parameters)
{
enum opahrs_result result;
portTickType lastSysTime;
GPSGoodUpdates = 0;
AhrsStatusData data;
AhrsStatusGet(&data);
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
// Main task loop
while (1) {
struct opahrs_msg_v1 rsp;
struct opahrs_msg_v1 req;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/* Spin here until we're in sync */
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
vTaskDelay(100 / portTICK_RATE_MS);
}
/* Give the other side a chance to keep up */
//vTaskDelay(100 / portTICK_RATE_MS);
if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
} else {
/* Comms error */
continue;
}
/* 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);
if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
struct opahrs_msg_v1 req;
load_magnetic_north(&(req.payload.user.v.req.north));
if ((result = PIOS_OPAHRS_SetMagNorth(&req)) == OPAHRS_RESULT_OK) {
HomeLocationIsUpdatedFlag = false;
data.HomeSet = AHRSSTATUS_HOMESET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
home_errors++;
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
AhrsStatusSet(&data);
break;
}
}
// Update calibration if necessary
AhrsStatusGet(&data);
if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE)) {
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
load_calibration(&(req.payload.user.v.req.calibration));
if ((result = PIOS_OPAHRS_SetGetCalibration(&req, &rsp)) == OPAHRS_RESULT_OK) {
update_calibration(&(rsp.payload.user.v.rsp.calibration));
AHRSCalibrationIsUpdatedFlag = false;
if (rsp.payload.user.v.req.calibration.measure_var != AHRS_ECHO)
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
calibration_errors++;
break;
}
}
// Update algorithm
if (AHRSSettingsIsUpdatedFlag || (data.AlgorithmSet == AHRSSTATUS_ALGORITHMSET_FALSE)) {
struct opahrs_msg_v1 req;
if (settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
req.payload.user.v.req.algorithm.algorithm = INSGPS_Algo;
if (settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE)
req.payload.user.v.req.algorithm.algorithm = SIMPLE_Algo;
if ((result = PIOS_OPAHRS_SetAlgorithm(&req)) == OPAHRS_RESULT_OK) {
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
AhrsStatusSet(&data);
algorithm_errors++;
break;
}
}
// If settings indicate, grab the raw and filtered data instead of estimate
if (settings.UpdateRaw) {
if ((result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
} else {
/* Comms error */
attituderaw_errors++;
break;
}
}
if (settings.UpdateFiltered) {
// Otherwise do standard technique
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
// Load barometer if updated
if (BaroAltitudeIsUpdatedFlag)
load_baro_altitude(&(req.payload.user.v.req.update));
else
req.payload.user.v.req.update.barometer.updated = 0;
// Load GPS if updated
if (GPSPositionIsUpdatedFlag)
load_gps_position(&(req.payload.user.v.req.update));
else
req.payload.user.v.req.update.gps.updated = 0;
// Transfer packet and process returned attitude
if ((result = PIOS_OPAHRS_SetGetUpdate(&req, &rsp)) == OPAHRS_RESULT_OK) {
if (req.payload.user.v.req.update.barometer.updated)
BaroAltitudeIsUpdatedFlag = false;
if (req.payload.user.v.req.update.gps.updated)
GPSPositionIsUpdatedFlag = false;
process_update(&(rsp.payload.user.v.rsp.update));
} else {
/* Comms error */
update_errors++;
break;
}
}
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
}
}
}
static void load_calibration(struct opahrs_msg_v1_req_calibration *calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
if (data.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET)
calibration->measure_var = AHRS_SET;
else if (data.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE)
calibration->measure_var = AHRS_MEASURE;
else
calibration->measure_var = AHRS_ECHO;
calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2];
calibration->accel_scale[0] = data.accel_scale[0];
calibration->accel_scale[1] = data.accel_scale[1];
calibration->accel_scale[2] = data.accel_scale[2];
calibration->accel_var[0] = data.accel_var[0];
calibration->accel_var[1] = data.accel_var[1];
calibration->accel_var[2] = data.accel_var[2];
calibration->gyro_bias[0] = data.gyro_bias[0];
calibration->gyro_bias[1] = data.gyro_bias[1];
calibration->gyro_bias[2] = data.gyro_bias[2];
calibration->gyro_scale[0] = data.gyro_scale[0];
calibration->gyro_scale[1] = data.gyro_scale[1];
calibration->gyro_scale[2] = data.gyro_scale[2];
calibration->gyro_var[0] = data.gyro_var[0];
calibration->gyro_var[1] = data.gyro_var[1];
calibration->gyro_var[2] = data.gyro_var[2];
calibration->mag_bias[0] = data.mag_bias[0];
calibration->mag_bias[1] = data.mag_bias[1];
calibration->mag_bias[2] = data.mag_bias[2];
calibration->mag_scale[0] = data.mag_scale[0];
calibration->mag_scale[1] = data.mag_scale[1];
calibration->mag_scale[2] = data.mag_scale[2];
calibration->mag_var[0] = data.mag_var[0];
calibration->mag_var[1] = data.mag_var[1];
calibration->mag_var[2] = data.mag_var[2];
}
static void update_calibration(struct opahrs_msg_v1_rsp_calibration *calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
AHRSCalibrationIsLocallyUpdateFlag = true;
data.accel_var[0] = calibration->accel_var[0];
data.accel_var[1] = calibration->accel_var[1];
data.accel_var[2] = calibration->accel_var[2];
data.gyro_var[0] = calibration->gyro_var[0];
data.gyro_var[1] = calibration->gyro_var[1];
data.gyro_var[2] = calibration->gyro_var[2];
data.mag_var[0] = calibration->mag_var[0];
data.mag_var[1] = calibration->mag_var[1];
data.mag_var[2] = calibration->mag_var[2];
AHRSCalibrationSet(&data);
}
static void load_magnetic_north(struct opahrs_msg_v1_req_north *mag_north)
{
HomeLocationData data;
HomeLocationGet(&data);
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_baro_altitude(struct opahrs_msg_v1_req_update *update)
{
BaroAltitudeData data;
BaroAltitudeGet(&data);
update->barometer.altitude = data.Altitude;
update->barometer.updated = TRUE;
}
static void load_gps_position(struct opahrs_msg_v1_req_update *update)
{
GPSPositionData data;
GPSPositionGet(&data);
HomeLocationData home;
HomeLocationGet(&home);
update->gps.updated = TRUE;
if (home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
PositionActualData pos;
PositionActualGet(&pos);
update->gps.NED[0] = 0;
update->gps.NED[1] = 0;
update->gps.NED[2] = 0;
update->gps.groundspeed = 0;
update->gps.heading = 0;
update->gps.quality = -1; // indicates indoor mode, high variance zeros update
} else {
// TODO: Parameterize these heuristics into the settings
if (data.Satellites >= 7 && data.PDOP < 3.5) {
if (GPSGoodUpdates < 30) {
GPSGoodUpdates++;
update->gps.quality = 0;
} else {
update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading;
double LLA[3] =
{ (double)data.Latitude / 1e7, (double)data.Longitude / 1e7, (double)(data.GeoidSeparation + data.Altitude) };
// convert from cm back to meters
double ECEF[3] = { (double)(home.ECEF[0] / 100), (double)(home.ECEF[1] / 100), (double)(home.ECEF[2] / 100) };
LLA2Base(LLA, ECEF, (float (*)[3])home.RNE, update->gps.NED);
update->gps.quality = 1;
}
} else {
GPSGoodUpdates = 0;
update->gps.quality = 0;
}
}
}
static void process_update(struct opahrs_msg_v1_rsp_update *update)
{
AttitudeActualData data;
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
data.q1 = update->quaternion.q1;
data.q2 = update->quaternion.q2;
data.q3 = update->quaternion.q3;
data.q4 = update->quaternion.q4;
float q[4] = { data.q1, data.q2, data.q3, data.q4 };
float rpy[3];
Quaternion2RPY(q, rpy);
data.Roll = rpy[0] - attitudeSettings.RollBias;
data.Pitch = rpy[1] - attitudeSettings.PitchBias;
data.Yaw = rpy[2];
if (data.Yaw < 0)
data.Yaw += 360;
AttitudeActualSet(&data);
PositionActualData pos;
PositionActualGet(&pos);
pos.North = update->NED[0];
pos.East = update->NED[1];
pos.Down = update->NED[2];
PositionActualSet(&pos);
VelocityActualData vel;
VelocityActualGet(&vel);
vel.North = update->Vel[0];
vel.East = update->Vel[1];
vel.Down = update->Vel[2];
VelocityActualSet(&vel);
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = update->load;
status.IdleTimePerCyle = update->idle_time;
status.RunningTimePerCyle = update->run_time;
status.DroppedUpdates = update->dropped_updates;
AhrsStatusSet(&status);
}
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw *attituderaw)
{
AttitudeRawData data;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_X] = attituderaw->mags.x;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y] = attituderaw->mags.y;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z] = attituderaw->mags.z;
data.gyros[ATTITUDERAW_GYROS_X] = attituderaw->gyros.x;
data.gyros[ATTITUDERAW_GYROS_Y] = attituderaw->gyros.y;
data.gyros[ATTITUDERAW_GYROS_Z] = attituderaw->gyros.z;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = attituderaw->gyros_filtered.x;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = attituderaw->gyros_filtered.y;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = attituderaw->gyros_filtered.z;
data.gyrotemp[ATTITUDERAW_GYROTEMP_XY] = attituderaw->gyros.xy_temp;
data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;
data.accels[ATTITUDERAW_ACCELS_X] = attituderaw->accels.x;
data.accels[ATTITUDERAW_ACCELS_Y] = attituderaw->accels.y;
data.accels[ATTITUDERAW_ACCELS_Z] = attituderaw->accels.z;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = attituderaw->accels_filtered.x;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = attituderaw->accels_filtered.y;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = attituderaw->accels_filtered.z;
AttitudeRawSet(&data);
}
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial *serial)
{
AhrsStatusData data;
// Get the current object data
AhrsStatusGet(&data);
for (uint8_t i = 0; i < sizeof(serial->serial_bcd); i++) {
data.SerialNumber[i] = serial->serial_bcd[i];
}
data.CommErrors[AHRSSTATUS_COMMERRORS_UPDATE] = update_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDERAW] = attituderaw_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ALGORITHM] = algorithm_errors;
AhrsStatusSet(&data);
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @brief Handles communication with AHRS and updating position
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file ahrs_comms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: As defined in PiOS/inc/pios_ahrs_comms.h
* Output objects: As defined in PiOS/inc/pios_ahrs_comms.h
*
* This module will periodically update the values of latest attitude solution
* and other objects that are transferred to and from the AHRS
* The module settings can configure how often AHRS is polled for a new solution.
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "ahrs_comms.h"
#include "ahrs_spi_comm.h"
// Private constants
#define STACK_SIZE 1400
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
// Private types
// Private variables
static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void* parameters);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AhrsInitComms();
// Start main task
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void* parameters)
{
portTickType lastSysTime;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/*Until AHRS connects, assume it doesn't know home */
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
//data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
// Main task loop
while (1)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
AhrsSendObjects();
AhrsCommStatus stat = AhrsGetStatus();
if(stat.linkOk)
{
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
}else
{
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
}
AhrsStatusData sData;
AhrsStatusGet(&sData);
sData.LinkRunning = stat.linkOk;
sData.AhrsKickstarts = stat.remote.kickStarts;
sData.AhrsCrcErrors = stat.remote.crcErrors;
sData.AhrsRetries = stat.remote.retries;
sData.AhrsInvalidPackets = stat.remote.invalidPacket;
sData.OpCrcErrors = stat.local.crcErrors;
sData.OpRetries = stat.local.retries;
sData.OpInvalidPackets = stat.local.invalidPacket;
AhrsStatusSet(&sData);
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
}
}
/**
* @}
* @}
*/

75
flight/OpenPilot/Modules/AHRSComms/inc/ahrs_comms.h
View File

@ -1,37 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @{
*
* @file ahrs_comms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_COMMS_H
#define AHRS_COMMS_H
#include "openpilot.h"
int32_t AHRSCommsInitialize(void);
#endif // AHRS_COMMS_H
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @{
*
* @file ahrs_comms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_COMMS_H
#define AHRS_COMMS_H
#include "openpilot.h"
int32_t AHRSCommsInitialize(void);
#endif // AHRS_COMMS_H

111
flight/OpenPilot/UAVObjects/ahrsstatus2.c
View File

@ -1,111 +0,0 @@
/**
******************************************************************************
* @addtogroup UAVObjects OpenPilot UAVObjects
* @{
* @addtogroup AhrsStatus2 AhrsStatus2
* @brief Status for the @ref AHRSCommsModule, including communication errors
*
* Autogenerated files and functions for AhrsStatus2 Object
* @{
*
* @file ahrsstatus2.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the AhrsStatus2 object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: ahrsstatus2.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "ahrsstatus2.h"
// Private variables
static UAVObjHandle handle;
// Private functions
static void setDefaults(UAVObjHandle obj, uint16_t instId);
/**
* Initialize object.
* \return 0 Success
* \return -1 Failure
*/
int32_t AhrsStatus2Initialize()
{
// Register object with the object manager
handle = UAVObjRegister(AHRSSTATUS2_OBJID, AHRSSTATUS2_NAME, AHRSSTATUS2_METANAME, 0,
AHRSSTATUS2_ISSINGLEINST, AHRSSTATUS2_ISSETTINGS, AHRSSTATUS2_NUMBYTES, &setDefaults);
// Done
if (handle != 0)
{
return 0;
}
else
{
return -1;
}
}
/**
* Initialize object fields and metadata with the default values.
* If a default value is not specified the object fields
* will be initialized to zero.
*/
static void setDefaults(UAVObjHandle obj, uint16_t instId)
{
AhrsStatus2Data data;
UAVObjMetadata metadata;
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(AhrsStatus2Data));
UAVObjSetInstanceData(obj, instId, &data);
// Initialize object metadata to their default values
metadata.access = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = 1000;
metadata.gcsTelemetryAcked = 0;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
metadata.loggingUpdatePeriod = 1000;
UAVObjSetMetadata(obj, &metadata);
}
/**
* Get object handle
*/
UAVObjHandle AhrsStatus2Handle()
{
return handle;
}
/**
* @}
*/

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

@ -41,7 +41,7 @@
#define AHRSSTATUS_H
// Object constants
#define AHRSSTATUS_OBJID 4061809000U
#define AHRSSTATUS_OBJID 188215176U
#define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1
@ -71,35 +71,33 @@
// Object data
typedef struct {
uint8_t SerialNumber[25];
uint8_t SerialNumber[8];
uint8_t CPULoad;
uint8_t IdleTimePerCyle;
uint8_t RunningTimePerCyle;
uint8_t DroppedUpdates;
uint8_t CommErrors[5];
uint8_t Initialized;
uint8_t AlgorithmSet;
uint8_t CalibrationSet;
uint8_t HomeSet;
uint8_t LinkRunning;
uint8_t AhrsKickstarts;
uint8_t AhrsCrcErrors;
uint8_t AhrsRetries;
uint8_t AhrsInvalidPackets;
uint8_t OpCrcErrors;
uint8_t OpRetries;
uint8_t OpInvalidPackets;
} __attribute__((packed)) AhrsStatusData;
// Field information
// Field SerialNumber information
/* Number of elements for field SerialNumber */
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 8
// Field CPULoad information
// Field IdleTimePerCyle information
// Field RunningTimePerCyle information
// Field DroppedUpdates information
// Field CommErrors information
/* Array element names for field CommErrors */
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;
@ -109,6 +107,16 @@ typedef enum { AHRSSTATUS_CALIBRATIONSET_FALSE=0, AHRSSTATUS_CALIBRATIONSET_TRUE
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { AHRSSTATUS_HOMESET_FALSE=0, AHRSSTATUS_HOMESET_TRUE=1 } AhrsStatusHomeSetOptions;
// Field LinkRunning information
/* Enumeration options for field LinkRunning */
typedef enum { AHRSSTATUS_LINKRUNNING_FALSE=0, AHRSSTATUS_LINKRUNNING_TRUE=1 } AhrsStatusLinkRunningOptions;
// Field AhrsKickstarts information
// Field AhrsCrcErrors information
// Field AhrsRetries information
// Field AhrsInvalidPackets information
// Field OpCrcErrors information
// Field OpRetries information
// Field OpInvalidPackets information
// Generic interface functions

131
flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h
View File

@ -1,131 +0,0 @@
/**
******************************************************************************
* @addtogroup UAVObjects OpenPilot UAVObjects
* @{
* @addtogroup AhrsStatus2 AhrsStatus2
* @brief Status for the @ref AHRSCommsModule, including communication errors
*
* Autogenerated files and functions for AhrsStatus2 Object
* @{
*
* @file ahrsstatus2.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the AhrsStatus2 object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: ahrsstatus2.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRSSTATUS2_H
#define AHRSSTATUS2_H
// Object constants
#define AHRSSTATUS2_OBJID 805003662U
#define AHRSSTATUS2_NAME "AhrsStatus2"
#define AHRSSTATUS2_METANAME "AhrsStatus2Meta"
#define AHRSSTATUS2_ISSINGLEINST 1
#define AHRSSTATUS2_ISSETTINGS 0
#define AHRSSTATUS2_NUMBYTES sizeof(AhrsStatus2Data)
// Object access macros
/**
* @function AhrsStatus2Get(dataOut)
* @brief Populate a AhrsStatus2Data object
* @param[out] dataOut
*/
#define AhrsStatus2Get(dataOut) UAVObjGetData(AhrsStatus2Handle(), dataOut)
#define AhrsStatus2Set(dataIn) UAVObjSetData(AhrsStatus2Handle(), dataIn)
#define AhrsStatus2InstGet(instId, dataOut) UAVObjGetInstanceData(AhrsStatus2Handle(), instId, dataOut)
#define AhrsStatus2InstSet(instId, dataIn) UAVObjSetInstanceData(AhrsStatus2Handle(), instId, dataIn)
#define AhrsStatus2ConnectQueue(queue) UAVObjConnectQueue(AhrsStatus2Handle(), queue, EV_MASK_ALL_UPDATES)
#define AhrsStatus2ConnectCallback(cb) UAVObjConnectCallback(AhrsStatus2Handle(), cb, EV_MASK_ALL_UPDATES)
#define AhrsStatus2CreateInstance() UAVObjCreateInstance(AhrsStatus2Handle())
#define AhrsStatus2RequestUpdate() UAVObjRequestUpdate(AhrsStatus2Handle())
#define AhrsStatus2RequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AhrsStatus2Handle(), instId)
#define AhrsStatus2Updated() UAVObjUpdated(AhrsStatus2Handle())
#define AhrsStatus2InstUpdated(instId) UAVObjUpdated(AhrsStatus2Handle(), instId)
#define AhrsStatus2GetMetadata(dataOut) UAVObjGetMetadata(AhrsStatus2Handle(), dataOut)
#define AhrsStatus2SetMetadata(dataIn) UAVObjSetMetadata(AhrsStatus2Handle(), dataIn)
#define AhrsStatus2ReadOnly(dataIn) UAVObjReadOnly(AhrsStatus2Handle())
// Object data
typedef struct {
uint8_t SerialNumber[8];
uint8_t CPULoad;
uint8_t IdleTimePerCyle;
uint8_t RunningTimePerCyle;
uint8_t DroppedUpdates;
uint8_t AlgorithmSet;
uint8_t CalibrationSet;
uint8_t HomeSet;
uint8_t LinkRunning;
uint8_t AhrsKickstarts;
uint8_t AhrsCrcErrors;
uint8_t AhrsRetries;
uint8_t AhrsInvalidPackets;
uint8_t OpCrcErrors;
uint8_t OpRetries;
uint8_t OpInvalidPackets;
} __attribute__((packed)) AhrsStatus2Data;
// Field information
// Field SerialNumber information
/* Number of elements for field SerialNumber */
#define AHRSSTATUS2_SERIALNUMBER_NUMELEM 8
// Field CPULoad information
// Field IdleTimePerCyle information
// Field RunningTimePerCyle information
// Field DroppedUpdates information
// Field AlgorithmSet information
/* Enumeration options for field AlgorithmSet */
typedef enum { AHRSSTATUS2_ALGORITHMSET_FALSE=0, AHRSSTATUS2_ALGORITHMSET_TRUE=1 } AhrsStatus2AlgorithmSetOptions;
// Field CalibrationSet information
/* Enumeration options for field CalibrationSet */
typedef enum { AHRSSTATUS2_CALIBRATIONSET_FALSE=0, AHRSSTATUS2_CALIBRATIONSET_TRUE=1 } AhrsStatus2CalibrationSetOptions;
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { AHRSSTATUS2_HOMESET_FALSE=0, AHRSSTATUS2_HOMESET_TRUE=1 } AhrsStatus2HomeSetOptions;
// Field LinkRunning information
/* Enumeration options for field LinkRunning */
typedef enum { AHRSSTATUS2_LINKRUNNING_FALSE=0, AHRSSTATUS2_LINKRUNNING_TRUE=1 } AhrsStatus2LinkRunningOptions;
// Field AhrsKickstarts information
// Field AhrsCrcErrors information
// Field AhrsRetries information
// Field AhrsInvalidPackets information
// Field OpCrcErrors information
// Field OpRetries information
// Field OpInvalidPackets information
// Generic interface functions
int32_t AhrsStatus2Initialize();
UAVObjHandle AhrsStatus2Handle();
#endif // AHRSSTATUS2_H
/**
* @}
* @}
*/

2
flight/OpenPilot/UAVObjects/uavobjectsinit.c
View File

@ -34,7 +34,6 @@
#include "ahrscalibration.h"
#include "ahrssettings.h"
#include "ahrsstatus.h"
#include "ahrsstatus2.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
#include "attituderaw.h"
@ -81,7 +80,6 @@ void UAVObjectsInitializeAll()
AHRSCalibrationInitialize();
AHRSSettingsInitialize();
AhrsStatusInitialize();
AhrsStatus2Initialize();
AttitudeActualInitialize();
AttitudeDesiredInitialize();
AttitudeRawInitialize();

1
flight/PiOS/Boards/STM32103CB_AHRS.h
View File

@ -27,7 +27,6 @@
#ifndef STM32103CB_AHRS_H_
#define STM32103CB_AHRS_H_
//------------------------
// Timers and Channels Used
//------------------------

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

@ -51,23 +51,6 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
SerialNumberElemNames.append("5");
SerialNumberElemNames.append("6");
SerialNumberElemNames.append("7");
SerialNumberElemNames.append("8");
SerialNumberElemNames.append("9");
SerialNumberElemNames.append("10");
SerialNumberElemNames.append("11");
SerialNumberElemNames.append("12");
SerialNumberElemNames.append("13");
SerialNumberElemNames.append("14");
SerialNumberElemNames.append("15");
SerialNumberElemNames.append("16");
SerialNumberElemNames.append("17");
SerialNumberElemNames.append("18");
SerialNumberElemNames.append("19");
SerialNumberElemNames.append("20");
SerialNumberElemNames.append("21");
SerialNumberElemNames.append("22");
SerialNumberElemNames.append("23");
SerialNumberElemNames.append("24");
fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) );
QStringList CPULoadElemNames;
CPULoadElemNames.append("0");
@ -81,19 +64,6 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
QStringList DroppedUpdatesElemNames;
DroppedUpdatesElemNames.append("0");
fields.append( new UAVObjectField(QString("DroppedUpdates"), QString("count"), UAVObjectField::UINT8, DroppedUpdatesElemNames, QStringList()) );
QStringList CommErrorsElemNames;
CommErrorsElemNames.append("Algorithm");
CommErrorsElemNames.append("Update");
CommErrorsElemNames.append("AttitudeRaw");
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;
@ -112,6 +82,33 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
HomeSetEnumOptions.append("FALSE");
HomeSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("HomeSet"), QString(""), UAVObjectField::ENUM, HomeSetElemNames, HomeSetEnumOptions) );
QStringList LinkRunningElemNames;
LinkRunningElemNames.append("0");
QStringList LinkRunningEnumOptions;
LinkRunningEnumOptions.append("FALSE");
LinkRunningEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("LinkRunning"), QString(""), UAVObjectField::ENUM, LinkRunningElemNames, LinkRunningEnumOptions) );
QStringList AhrsKickstartsElemNames;
AhrsKickstartsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsKickstarts"), QString("count"), UAVObjectField::UINT8, AhrsKickstartsElemNames, QStringList()) );
QStringList AhrsCrcErrorsElemNames;
AhrsCrcErrorsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsCrcErrors"), QString("count"), UAVObjectField::UINT8, AhrsCrcErrorsElemNames, QStringList()) );
QStringList AhrsRetriesElemNames;
AhrsRetriesElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsRetries"), QString("count"), UAVObjectField::UINT8, AhrsRetriesElemNames, QStringList()) );
QStringList AhrsInvalidPacketsElemNames;
AhrsInvalidPacketsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsInvalidPackets"), QString("count"), UAVObjectField::UINT8, AhrsInvalidPacketsElemNames, QStringList()) );
QStringList OpCrcErrorsElemNames;
OpCrcErrorsElemNames.append("0");
fields.append( new UAVObjectField(QString("OpCrcErrors"), QString("count"), UAVObjectField::UINT8, OpCrcErrorsElemNames, QStringList()) );
QStringList OpRetriesElemNames;
OpRetriesElemNames.append("0");
fields.append( new UAVObjectField(QString("OpRetries"), QString("count"), UAVObjectField::UINT8, OpRetriesElemNames, QStringList()) );
QStringList OpInvalidPacketsElemNames;
OpInvalidPacketsElemNames.append("0");
fields.append( new UAVObjectField(QString("OpInvalidPackets"), QString("count"), UAVObjectField::UINT8, OpInvalidPacketsElemNames, QStringList()) );
// Initialize object
initializeFields(fields, (quint8*)&data, NUMBYTES);

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

@ -43,35 +43,33 @@ class UAVOBJECTS_EXPORT AhrsStatus: public UAVDataObject
public:
// Field structure
typedef struct {
quint8 SerialNumber[25];
quint8 SerialNumber[8];
quint8 CPULoad;
quint8 IdleTimePerCyle;
quint8 RunningTimePerCyle;
quint8 DroppedUpdates;
quint8 CommErrors[5];
quint8 Initialized;
quint8 AlgorithmSet;
quint8 CalibrationSet;
quint8 HomeSet;
quint8 LinkRunning;
quint8 AhrsKickstarts;
quint8 AhrsCrcErrors;
quint8 AhrsRetries;
quint8 AhrsInvalidPackets;
quint8 OpCrcErrors;
quint8 OpRetries;
quint8 OpInvalidPackets;
} __attribute__((packed)) DataFields;
// Field information
// Field SerialNumber information
/* Number of elements for field SerialNumber */
static const quint32 SERIALNUMBER_NUMELEM = 25;
static const quint32 SERIALNUMBER_NUMELEM = 8;
// Field CPULoad information
// Field IdleTimePerCyle information
// Field RunningTimePerCyle information
// Field DroppedUpdates information
// Field CommErrors information
/* Array element names for field CommErrors */
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;
@ -81,10 +79,20 @@ public:
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions;
// Field LinkRunning information
/* Enumeration options for field LinkRunning */
typedef enum { LINKRUNNING_FALSE=0, LINKRUNNING_TRUE=1 } LinkRunningOptions;
// Field AhrsKickstarts information
// Field AhrsCrcErrors information
// Field AhrsRetries information
// Field AhrsInvalidPackets information
// Field OpCrcErrors information
// Field OpRetries information
// Field OpInvalidPackets information
// Constants
static const quint32 OBJID = 4061809000U;
static const quint32 OBJID = 188215176U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0;

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

@ -40,7 +40,7 @@ _fields = [ \
uavobject.UAVObjectField(
'SerialNumber',
'B',
25,
8,
[
'0',
'1',
@ -50,23 +50,6 @@ _fields = [ \
'5',
'6',
'7',
'8',
'9',
'10',
'11',
'12',
'13',
'14',
'15',
'16',
'17',
'18',
'19',
'20',
'21',
'22',
'23',
'24',
],
{
}
@ -111,32 +94,6 @@ _fields = [ \
{
}
),
uavobject.UAVObjectField(
'CommErrors',
'B',
5,
[
'Algorithm',
'Update',
'AttitudeRaw',
'HomeLocation',
'Calibration',
],
{
}
),
uavobject.UAVObjectField(
'Initialized',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'AlgorithmSet',
'b',
@ -173,12 +130,94 @@ _fields = [ \
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'LinkRunning',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'AhrsKickstarts',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsCrcErrors',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsRetries',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsInvalidPackets',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpCrcErrors',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpRetries',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpInvalidPackets',
'B',
1,
[
'0',
],
{
}
),
]
class AhrsStatus(uavobject.UAVObject):
## Object constants
OBJID = 4061809000
OBJID = 188215176
NAME = "AhrsStatus"
METANAME = "AhrsStatusMeta"
ISSINGLEINST = 1

192
ground/src/plugins/uavobjects/ahrsstatus2.cpp
View File

@ -1,192 +0,0 @@
/**
******************************************************************************
*
* @file ahrsstatus2.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectsPlugin UAVObjects Plugin
* @{
*
* @note Object definition file: ahrsstatus2.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @brief The UAVUObjects GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "ahrsstatus2.h"
#include "uavobjectfield.h"
const QString AhrsStatus2::NAME = QString("AhrsStatus2");
/**
* Constructor
*/
AhrsStatus2::AhrsStatus2(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
{
// Create fields
QList<UAVObjectField*> fields;
QStringList SerialNumberElemNames;
SerialNumberElemNames.append("0");
SerialNumberElemNames.append("1");
SerialNumberElemNames.append("2");
SerialNumberElemNames.append("3");
SerialNumberElemNames.append("4");
SerialNumberElemNames.append("5");
SerialNumberElemNames.append("6");
SerialNumberElemNames.append("7");
fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) );
QStringList CPULoadElemNames;
CPULoadElemNames.append("0");
fields.append( new UAVObjectField(QString("CPULoad"), QString("count"), UAVObjectField::UINT8, CPULoadElemNames, QStringList()) );
QStringList IdleTimePerCyleElemNames;
IdleTimePerCyleElemNames.append("0");
fields.append( new UAVObjectField(QString("IdleTimePerCyle"), QString("10x ms"), UAVObjectField::UINT8, IdleTimePerCyleElemNames, QStringList()) );
QStringList RunningTimePerCyleElemNames;
RunningTimePerCyleElemNames.append("0");
fields.append( new UAVObjectField(QString("RunningTimePerCyle"), QString("10x ms"), UAVObjectField::UINT8, RunningTimePerCyleElemNames, QStringList()) );
QStringList DroppedUpdatesElemNames;
DroppedUpdatesElemNames.append("0");
fields.append( new UAVObjectField(QString("DroppedUpdates"), QString("count"), UAVObjectField::UINT8, DroppedUpdatesElemNames, QStringList()) );
QStringList AlgorithmSetElemNames;
AlgorithmSetElemNames.append("0");
QStringList AlgorithmSetEnumOptions;
AlgorithmSetEnumOptions.append("FALSE");
AlgorithmSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("AlgorithmSet"), QString(""), UAVObjectField::ENUM, AlgorithmSetElemNames, AlgorithmSetEnumOptions) );
QStringList CalibrationSetElemNames;
CalibrationSetElemNames.append("0");
QStringList CalibrationSetEnumOptions;
CalibrationSetEnumOptions.append("FALSE");
CalibrationSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("CalibrationSet"), QString(""), UAVObjectField::ENUM, CalibrationSetElemNames, CalibrationSetEnumOptions) );
QStringList HomeSetElemNames;
HomeSetElemNames.append("0");
QStringList HomeSetEnumOptions;
HomeSetEnumOptions.append("FALSE");
HomeSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("HomeSet"), QString(""), UAVObjectField::ENUM, HomeSetElemNames, HomeSetEnumOptions) );
QStringList LinkRunningElemNames;
LinkRunningElemNames.append("0");
QStringList LinkRunningEnumOptions;
LinkRunningEnumOptions.append("FALSE");
LinkRunningEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("LinkRunning"), QString(""), UAVObjectField::ENUM, LinkRunningElemNames, LinkRunningEnumOptions) );
QStringList AhrsKickstartsElemNames;
AhrsKickstartsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsKickstarts"), QString("count"), UAVObjectField::UINT8, AhrsKickstartsElemNames, QStringList()) );
QStringList AhrsCrcErrorsElemNames;
AhrsCrcErrorsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsCrcErrors"), QString("count"), UAVObjectField::UINT8, AhrsCrcErrorsElemNames, QStringList()) );
QStringList AhrsRetriesElemNames;
AhrsRetriesElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsRetries"), QString("count"), UAVObjectField::UINT8, AhrsRetriesElemNames, QStringList()) );
QStringList AhrsInvalidPacketsElemNames;
AhrsInvalidPacketsElemNames.append("0");
fields.append( new UAVObjectField(QString("AhrsInvalidPackets"), QString("count"), UAVObjectField::UINT8, AhrsInvalidPacketsElemNames, QStringList()) );
QStringList OpCrcErrorsElemNames;
OpCrcErrorsElemNames.append("0");
fields.append( new UAVObjectField(QString("OpCrcErrors"), QString("count"), UAVObjectField::UINT8, OpCrcErrorsElemNames, QStringList()) );
QStringList OpRetriesElemNames;
OpRetriesElemNames.append("0");
fields.append( new UAVObjectField(QString("OpRetries"), QString("count"), UAVObjectField::UINT8, OpRetriesElemNames, QStringList()) );
QStringList OpInvalidPacketsElemNames;
OpInvalidPacketsElemNames.append("0");
fields.append( new UAVObjectField(QString("OpInvalidPackets"), QString("count"), UAVObjectField::UINT8, OpInvalidPacketsElemNames, QStringList()) );
// Initialize object
initializeFields(fields, (quint8*)&data, NUMBYTES);
// Set the default field values
setDefaultFieldValues();
}
/**
* Get the default metadata for this object
*/
UAVObject::Metadata AhrsStatus2::getDefaultMetadata()
{
UAVObject::Metadata metadata;
metadata.flightAccess = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.gcsTelemetryAcked = 0;
metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.flightTelemetryAcked = 0;
metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
metadata.flightTelemetryUpdatePeriod = 1000;
metadata.loggingUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
metadata.loggingUpdatePeriod = 1000;
return metadata;
}
/**
* Initialize object fields with the default values.
* If a default value is not specified the object fields
* will be initialized to zero.
*/
void AhrsStatus2::setDefaultFieldValues()
{
}
/**
* Get the object data fields
*/
AhrsStatus2::DataFields AhrsStatus2::getData()
{
QMutexLocker locker(mutex);
return data;
}
/**
* Set the object data fields
*/
void AhrsStatus2::setData(const DataFields& data)
{
QMutexLocker locker(mutex);
// Get metadata
Metadata mdata = getMetadata();
// Update object if the access mode permits
if ( mdata.gcsAccess == ACCESS_READWRITE )
{
this->data = data;
emit objectUpdatedAuto(this); // trigger object updated event
emit objectUpdated(this);
}
}
/**
* Create a clone of this object, a new instance ID must be specified.
* Do not use this function directly to create new instances, the
* UAVObjectManager should be used instead.
*/
UAVDataObject* AhrsStatus2::clone(quint32 instID)
{
AhrsStatus2* obj = new AhrsStatus2();
obj->initialize(instID, this->getMetaObject());
return obj;
}
/**
* Static function to retrieve an instance of the object.
*/
AhrsStatus2* AhrsStatus2::GetInstance(UAVObjectManager* objMngr, quint32 instID)
{
return dynamic_cast<AhrsStatus2*>(objMngr->getObject(AhrsStatus2::OBJID, instID));
}

118
ground/src/plugins/uavobjects/ahrsstatus2.h
View File

@ -1,118 +0,0 @@
/**
******************************************************************************
*
* @file ahrsstatus2.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectsPlugin UAVObjects Plugin
* @{
*
* @note Object definition file: ahrsstatus2.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @brief The UAVUObjects GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRSSTATUS2_H
#define AHRSSTATUS2_H
#include "uavdataobject.h"
#include "uavobjectmanager.h"
class UAVOBJECTS_EXPORT AhrsStatus2: public UAVDataObject
{
Q_OBJECT
public:
// Field structure
typedef struct {
quint8 SerialNumber[8];
quint8 CPULoad;
quint8 IdleTimePerCyle;
quint8 RunningTimePerCyle;
quint8 DroppedUpdates;
quint8 AlgorithmSet;
quint8 CalibrationSet;
quint8 HomeSet;
quint8 LinkRunning;
quint8 AhrsKickstarts;
quint8 AhrsCrcErrors;
quint8 AhrsRetries;
quint8 AhrsInvalidPackets;
quint8 OpCrcErrors;
quint8 OpRetries;
quint8 OpInvalidPackets;
} __attribute__((packed)) DataFields;
// Field information
// Field SerialNumber information
/* Number of elements for field SerialNumber */
static const quint32 SERIALNUMBER_NUMELEM = 8;
// Field CPULoad information
// Field IdleTimePerCyle information
// Field RunningTimePerCyle information
// Field DroppedUpdates information
// Field AlgorithmSet information
/* Enumeration options for field AlgorithmSet */
typedef enum { ALGORITHMSET_FALSE=0, ALGORITHMSET_TRUE=1 } AlgorithmSetOptions;
// Field CalibrationSet information
/* Enumeration options for field CalibrationSet */
typedef enum { CALIBRATIONSET_FALSE=0, CALIBRATIONSET_TRUE=1 } CalibrationSetOptions;
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions;
// Field LinkRunning information
/* Enumeration options for field LinkRunning */
typedef enum { LINKRUNNING_FALSE=0, LINKRUNNING_TRUE=1 } LinkRunningOptions;
// Field AhrsKickstarts information
// Field AhrsCrcErrors information
// Field AhrsRetries information
// Field AhrsInvalidPackets information
// Field OpCrcErrors information
// Field OpRetries information
// Field OpInvalidPackets information
// Constants
static const quint32 OBJID = 805003662U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0;
static const quint32 NUMBYTES = sizeof(DataFields);
// Functions
AhrsStatus2();
DataFields getData();
void setData(const DataFields& data);
Metadata getDefaultMetadata();
UAVDataObject* clone(quint32 instID);
static AhrsStatus2* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
private:
DataFields data;
void setDefaultFieldValues();
};
#endif // AHRSSTATUS2_H

251
ground/src/plugins/uavobjects/ahrsstatus2.py
View File

@ -1,251 +0,0 @@
##
##############################################################################
#
# @file ahrsstatus2.py
# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
# @brief Implementation of the AhrsStatus2 object. This file has been
# automatically generated by the UAVObjectGenerator.
#
# @note Object definition file: ahrsstatus2.xml.
# This is an automatically generated file.
# DO NOT modify manually.
#
# @see The GNU Public License (GPL) Version 3
#
#############################################################################/
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#
import uavobject
import struct
from collections import namedtuple
# This is a list of instances of the data fields contained in this object
_fields = [ \
uavobject.UAVObjectField(
'SerialNumber',
'B',
8,
[
'0',
'1',
'2',
'3',
'4',
'5',
'6',
'7',
],
{
}
),
uavobject.UAVObjectField(
'CPULoad',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'IdleTimePerCyle',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'RunningTimePerCyle',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'DroppedUpdates',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AlgorithmSet',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'CalibrationSet',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'HomeSet',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'LinkRunning',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'AhrsKickstarts',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsCrcErrors',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsRetries',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'AhrsInvalidPackets',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpCrcErrors',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpRetries',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'OpInvalidPackets',
'B',
1,
[
'0',
],
{
}
),
]
class AhrsStatus2(uavobject.UAVObject):
## Object constants
OBJID = 805003662
NAME = "AhrsStatus2"
METANAME = "AhrsStatus2Meta"
ISSINGLEINST = 1
ISSETTINGS = 0
def __init__(self):
uavobject.UAVObject.__init__(self,
self.OBJID,
self.NAME,
self.METANAME,
0,
self.ISSINGLEINST)
for f in _fields:
self.add_field(f)
def __str__(self):
s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n"
% (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
for f in self.get_tuple()._fields:
s += ("\t%s\n" % f)
return (s)
def main():
# Instantiate the object and dump out some interesting info
x = AhrsStatus2()
print (x)
if __name__ == "__main__":
#import pdb ; pdb.run('main()')
main()

6
ground/src/plugins/uavobjects/uavobjects.pro
View File

@ -48,8 +48,7 @@ HEADERS += uavobjects_global.h \
velocityactual.h \
guidancesettings.h \
positiondesired.h \
attitudesettings.h \
ahrsstatus2.h
attitudesettings.h
SOURCES += uavobject.cpp \
uavmetaobject.cpp \
@ -95,6 +94,5 @@ SOURCES += uavobject.cpp \
velocityactual.cpp \
guidancesettings.cpp \
positiondesired.cpp \
attitudesettings.cpp \
ahrsstatus2.cpp
attitudesettings.cpp
OTHER_FILES += UAVObjects.pluginspec

2
ground/src/plugins/uavobjects/uavobjectsinit.cpp
View File

@ -36,7 +36,6 @@
#include "ahrscalibration.h"
#include "ahrssettings.h"
#include "ahrsstatus.h"
#include "ahrsstatus2.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
#include "attituderaw.h"
@ -83,7 +82,6 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr)
objMngr->registerObject( new AHRSCalibration() );
objMngr->registerObject( new AHRSSettings() );
objMngr->registerObject( new AhrsStatus() );
objMngr->registerObject( new AhrsStatus2() );
objMngr->registerObject( new AttitudeActual() );
objMngr->registerObject( new AttitudeDesired() );
objMngr->registerObject( new AttitudeRaw() );

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

@ -1,19 +1,27 @@
<xml>
<object name="AhrsStatus" singleinstance="true" settings="false">
<description>Status for the @ref AHRSCommsModule, including communication errors</description>
<field name="SerialNumber" units="n/a" type="uint8" elements="25"/>
<field name="CPULoad" units="count" type="uint8" elements="1"/>
<field name="IdleTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<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"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>
<xml>
<object name="AhrsStatus" singleinstance="true" settings="false">
<description>Status for the @ref AHRSCommsModule, including communication errors</description>
<field name="SerialNumber" units="n/a" type="uint8" elements="8"/>
<field name="CPULoad" units="count" type="uint8" elements="1"/>
<field name="IdleTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<field name="RunningTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<field name="DroppedUpdates" units="count" type="uint8" elements="1"/>
<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"/>
<field name="LinkRunning" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="AhrsKickstarts" units="count" type="uint8" elements="1"/>
<field name="AhrsCrcErrors" units="count" type="uint8" elements="1"/>
<field name="AhrsRetries" units="count" type="uint8" elements="1"/>
<field name="AhrsInvalidPackets" units="count" type="uint8" elements="1"/>
<field name="OpCrcErrors" units="count" type="uint8" elements="1"/>
<field name="OpRetries" units="count" type="uint8" elements="1"/>
<field name="OpInvalidPackets" units="count" type="uint8" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>

27
ground/src/shared/uavobjectdefinition/ahrsstatus2.xml
View File

@ -1,27 +0,0 @@
<xml>
<object name="AhrsStatus2" singleinstance="true" settings="false">
<description>Status for the @ref AHRSCommsModule, including communication errors</description>
<field name="SerialNumber" units="n/a" type="uint8" elements="8"/>
<field name="CPULoad" units="count" type="uint8" elements="1"/>
<field name="IdleTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<field name="RunningTimePerCyle" units="10x ms" type="uint8" elements="1"/>
<field name="DroppedUpdates" units="count" type="uint8" elements="1"/>
<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"/>
<field name="LinkRunning" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="AhrsKickstarts" units="count" type="uint8" elements="1"/>
<field name="AhrsCrcErrors" units="count" type="uint8" elements="1"/>
<field name="AhrsRetries" units="count" type="uint8" elements="1"/>
<field name="AhrsInvalidPackets" units="count" type="uint8" elements="1"/>
<field name="OpCrcErrors" units="count" type="uint8" elements="1"/>
<field name="OpRetries" units="count" type="uint8" elements="1"/>
<field name="OpInvalidPackets" units="count" type="uint8" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>