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

AHRS: Added UAVObject based SPI communication between AHRS and OP.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1830 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
les 2010-10-01 17:31:13 +00:00 committed by les
parent 498f3d0fa6
commit b2d56ca34c
16 changed files with 2072 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

858
flight/AHRS/ahrs_newspi.c Normal file
View File

@ -0,0 +1,858 @@
/**
******************************************************************************
* @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];
//! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE;
//! 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 (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];
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];
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
/**
* @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){
calibration_pending = true;
}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;
}
}
/**
* @}
*/

138
flight/Libraries/ahrs_comm_objects.c Normal file
View File

@ -0,0 +1,138 @@
#include "ahrs_spi_comm.h"
#include "pios_debug.h"
static AttitudeRawData AttitudeRaw;
static AttitudeActualData AttitudeActual;
static AHRSCalibrationData AHRSCalibration;
static AttitudeSettingsData AttitudeSettings;
static AhrsStatus2Data AhrsStatus2;
static BaroAltitudeData BaroAltitude;
static GPSPositionData GPSPosition;
static PositionActualData PositionActual;
static HomeLocationData HomeLocation;
static AHRSSettingsData AHRSSettings;
AhrsSharedObject objectHandles[MAX_AHRS_OBJECTS];
#ifndef NULL
#define NULL ((void *)0)
#endif
#ifdef IN_AHRS
//slightly hacky - implement our own version of the xxxHandle() functions
#define CREATEHANDLE(n,obj) \
void * obj##Handle() {return (&objectHandles[n]);}
CREATEHANDLE( 0, AttitudeRaw );
CREATEHANDLE( 1, AttitudeActual );
CREATEHANDLE( 2, AHRSCalibration );
CREATEHANDLE( 3, AttitudeSettings );
CREATEHANDLE( 4, AhrsStatus2 );
CREATEHANDLE( 5, BaroAltitude );
CREATEHANDLE( 6, GPSPosition );
CREATEHANDLE( 7, PositionActual );
CREATEHANDLE( 8, HomeLocation );
CREATEHANDLE( 9, AHRSSettings );
#if 10 != MAX_AHRS_OBJECTS //sanity check
#error We did not create the correct number of xxxHandle() functions
#endif
#define ADDHANDLE(idx,obj) {\
int n = idx;\
objectHandles[n].data = &obj;\
objectHandles[n].size = sizeof(obj);\
objectHandles[n].index = n;\
}
#else
static void ObjectUpdatedCb(UAVObjEvent * ev);
#define ADDHANDLE(idx,obj) {\
int n = idx;\
objectHandles[n].data = &obj;\
objectHandles[n].uavHandle = obj##Handle();\
objectHandles[n].size = sizeof(obj);\
objectHandles[n].index = n;\
}
#endif
void AhrsInitHandles( void )
{
int idx = 0;
//Note: the first item in this list has the highest priority
//the last has the lowest priority
ADDHANDLE( idx++, AttitudeRaw );
ADDHANDLE( idx++, AttitudeActual );
ADDHANDLE( idx++, AHRSCalibration );
ADDHANDLE( idx++, AttitudeSettings );
ADDHANDLE( idx++, AhrsStatus2 );
ADDHANDLE( idx++, BaroAltitude );
ADDHANDLE( idx++, GPSPosition );
ADDHANDLE( idx++, PositionActual );
ADDHANDLE( idx++, HomeLocation );
ADDHANDLE( idx++, AHRSSettings );
if( idx != MAX_AHRS_OBJECTS ) {
PIOS_DEBUG_Assert( 0 );
}
//Note: Only connect objects that the AHRS needs to read
//When the AHRS writes to these the data does a round trip
//AHRS->OP->AHRS due to these events
#ifndef IN_AHRS
AHRSSettingsConnectCallback(ObjectUpdatedCb);
BaroAltitudeConnectCallback(ObjectUpdatedCb);
GPSPositionConnectCallback(ObjectUpdatedCb);
HomeLocationConnectCallback(ObjectUpdatedCb);
AHRSCalibrationConnectCallback(ObjectUpdatedCb);
#endif
}
AhrsObjHandle AhrsFromIndex( uint8_t index )
{
if( index >= MAX_AHRS_OBJECTS ) {
return( NULL );
}
return( &objectHandles[index] );
}
#ifndef IN_AHRS
AhrsObjHandle AhrsFromUAV( UAVObjHandle obj )
{
if( objectHandles[0].uavHandle == NULL ) { //Oops - we haven't been initialised!
PIOS_DEBUG_Assert( 0 );
}
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
if( objectHandles[ct].uavHandle == obj ) {
return( &objectHandles[ct] );
}
}
return( NULL );
}
/** Callback to update AHRS from UAVObjects
*/
static void ObjectUpdatedCb(UAVObjEvent * ev)
{
if(!(ev->event & EV_MASK_ALL_UPDATES))
{
return;
}
AhrsObjHandle hdl = AhrsFromUAV(ev->obj);
if(hdl)
{
AhrsSharedData data; //this is guaranteed to be big enough
UAVObjGetData(ev->obj,&data);
AhrsSetData(hdl, &data);
}
}
#endif

421
flight/Libraries/ahrs_spi_comm.c Normal file
View File

@ -0,0 +1,421 @@
#include "ahrs_spi_comm.h"
#include "ahrs_spi_program.h"
#ifdef IN_AHRS
#include <string.h>
#include "pios_debug.h"
#include "pios_spi.h"
#include "pios_irq.h"
#include "ahrs_spi_program_slave.h"
#include "STM32103CB_AHRS.h"
#endif
/*transmit and receive packet magic numbers.
These numbers are chosen to be very unlikely to occur due to noise.
CRC8 does not always catch noise from cross-coupling between data lines.
*/
#ifdef IN_AHRS
#define TXMAGIC 0xA55AF0C3
#define RXMAGIC 0x3C5AA50F
#else
#define RXMAGIC 0xA55AF0C3
#define TXMAGIC 0x3C5AA50F
#endif
//packet types
typedef enum {COMMS_NULL, COMMS_OBJECT, COMMS_PROGRAM} COMMSCOMMAND;
//The maximum number of objects that can be updated in one cycle.
//Currently the link is capable of sending 3 packets per cycle but 2 is enough
#define MAX_UPDATE_OBJECTS 3
//Number of transmissions + 1 before we expect to see the data acknowledge
//This is controlled by the SPI hardware.
#define ACK_LATENCY 4
/** All data for one object
*/
typedef struct {
uint8_t done;
uint8_t index;
AhrsSharedData object;
} ObjectPacketData;
/** One complete packet.
Other packet types are allowed for. The frame size will be the size of this
structure.
*/
typedef struct {
uint32_t magicNumber;
COMMSCOMMAND command;
AhrsEndStatus status;
union { //allow for expansion to other packet types.
ObjectPacketData objects[MAX_UPDATE_OBJECTS];
};
} CommsDataPacket;
static void FillObjectPacket();
static void CommsCallback( uint8_t crc_ok, uint8_t crc_val );
static void SetObjectDirty( const int idx );
static void HandleObjectPacket();
static void HandleRxPacket();
static void PollEvents();
#ifndef IN_AHRS
static void SendPacket( void );
static void AhrsUpdatedCb( AhrsObjHandle handle );
#endif
/** Transmit data buffer
*/
static CommsDataPacket txPacket;
/** Receive data buffer
*/
static CommsDataPacket rxPacket;
/** Objects that have changed and so should be transmitted
*/
static unsigned int dirtyObjects[MAX_AHRS_OBJECTS];
/** Objects that have been updated at startup
*/
static bool readyObjects[MAX_AHRS_OBJECTS];
/** List of event callbacks
*/
static AhrsEventCallback objCallbacks[MAX_AHRS_OBJECTS];
/** Pending events.
*/
static bool callbackPending[MAX_AHRS_OBJECTS];
//More than this number of errors in a row will indicate the link is down
#define MAX_CRC_ERRORS 50
//At least this number of good frames are needed to indicate the link is up.
#define MIN_OK_FRAMES 50
//At least this number of empty objects are needed before the initial flood of events is over.
#define MIN_EMPTY_OBJECTS 10
static uint8_t linkOk = false;
static int okCount = MIN_OK_FRAMES;
static int emptyCount = MIN_EMPTY_OBJECTS;
void AhrsInitComms( void )
{
AhrsInitHandles();
memset( objCallbacks, 0, sizeof( AhrsEventCallback ) * MAX_AHRS_OBJECTS );
memset( callbackPending, 0, sizeof( bool ) * MAX_AHRS_OBJECTS );
memset( dirtyObjects, 0, sizeof( unsigned int ) * MAX_AHRS_OBJECTS );
memset( &txPacket, 0, sizeof( txPacket ) );
memset( &rxPacket, 0, sizeof( rxPacket ) );
memset(&readyObjects,0,sizeof( bool ) * MAX_AHRS_OBJECTS );
txPacket.command = COMMS_NULL;
rxPacket.command = COMMS_NULL;
#ifdef IN_AHRS
PIOS_SPI_Init();
#else
PIOS_SPI_SetClockSpeed( PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8 ); //72MHz/16 = 4.5MHz
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
AhrsObjHandle hdl = AhrsFromIndex( ct );
if( hdl) {
AhrsConnectCallBack( hdl, AhrsUpdatedCb );
}
}
#endif
}
int32_t AhrsSetData( AhrsObjHandle obj, const void *dataIn )
{
if( obj == NULL || dataIn == NULL || obj->data == NULL ) {
return( -1 );
}
if( memcmp( obj->data, dataIn, obj->size ) == 0 ) { //nothing to do, don't generate an event
return( 0 );
}
memcpy( obj->data, dataIn, obj->size );
SetObjectDirty( obj->index );
return( 0 );
}
int32_t AhrsGetData( AhrsObjHandle obj, void *dataOut )
{
if( obj == NULL || dataOut == NULL || obj->data == NULL ) {
return( -1 );
}
memcpy( dataOut, obj->data, obj->size );
return( 0 );
}
/** Mark an object to be sent
*/
void SetObjectDirty( const int idx )
{
if( idx < 0 || idx >= MAX_AHRS_OBJECTS ) {
return;
}
dirtyObjects[idx] = ACK_LATENCY;
}
/** Work out what data needs to be sent.
If an object was not sent it will be retried 4 frames later
*/
void FillObjectPacket()
{
txPacket.command = COMMS_OBJECT;
txPacket.magicNumber = TXMAGIC;
int idx = 0;
for( int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++ ) {
txPacket.objects[ct].index = AHRS_NO_OBJECT;
for( ; idx < MAX_AHRS_OBJECTS; idx++ ) {
if( dirtyObjects[idx] > 0 ) {
if( dirtyObjects[idx] == ACK_LATENCY ) {
dirtyObjects[idx]--;
txPacket.objects[ct].index = idx;
AhrsObjHandle hdl = AhrsFromIndex( idx );
if( hdl ) {
memcpy( &txPacket.objects[ct].object,
hdl->data,
hdl->size );
}
break;
} else {
dirtyObjects[idx] --;
if( dirtyObjects[idx] == 0 ) { //timed out
dirtyObjects[idx] = ACK_LATENCY;
txPacket.status.retries++;
}
}
}
}
}
}
/** Process a received packet
*/
void HandleRxPacket()
{
switch( rxPacket.command ) {
case COMMS_NULL:
break;
case COMMS_OBJECT:
HandleObjectPacket();
break;
case COMMS_PROGRAM: //TODO: programming protocol
break;
default:
txPacket.status.invalidPacket++;
}
}
/** Process a received UAVObject packet
*/
void HandleObjectPacket()
{
for( int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++ ) {
//Flag objects that have been successfully received at the other end
uint8_t idx = rxPacket.objects[ct].done;
if( idx < MAX_AHRS_OBJECTS ) {
if( dirtyObjects[idx] == 1 ) { //this ack is the correct one for the last send
dirtyObjects[idx] = 0;
}
}
txPacket.objects[ct].done = AHRS_NO_OBJECT;
idx = rxPacket.objects[ct].index;
if( idx == AHRS_NO_OBJECT ) {
if( emptyCount > 0 ) {
emptyCount--;
}
continue;
}
AhrsObjHandle obj = AhrsFromIndex( idx );
if( obj ) {
memcpy( obj->data, &rxPacket.objects[ct].object, obj->size );
txPacket.objects[ct].done = idx;
callbackPending[idx] = true;
readyObjects[idx] = true;
} else {
txPacket.status.invalidPacket++;
}
}
#ifdef IN_AHRS
FillObjectPacket(); //ready for the next frame
#endif
}
int32_t AhrsConnectCallBack( AhrsObjHandle obj, AhrsEventCallback cb )
{
if( obj == NULL || obj->data == NULL ) {
return( -1 );
}
objCallbacks[obj->index] = cb;
return( 0 );
}
AhrsCommStatus AhrsGetStatus()
{
AhrsCommStatus status;
status.remote = rxPacket.status;
status.local = txPacket.status;
status.linkOk = linkOk;
return( status );
}
void CommsCallback( uint8_t crc_ok, uint8_t crc_val )
{
#ifndef IN_AHRS
PIOS_SPI_RC_PinSet( PIOS_OPAHRS_SPI, 1 ); //signal the end of the transfer
#endif
txPacket.command = COMMS_NULL; //we must send something so default to null
if(rxPacket.magicNumber != RXMAGIC)
{
crc_ok = false;
}
rxPacket.magicNumber = 0;
if( crc_ok ) {
if(!linkOk && okCount > 0 ) {
okCount--;
if( okCount == 0 ) {
linkOk = true;
okCount = MAX_CRC_ERRORS;
emptyCount = MIN_EMPTY_OBJECTS;
}
}
HandleRxPacket();
} else {
#ifdef IN_AHRS //AHRS - do we neeed to enter program mode?
if( memcmp( &rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH ) == 0 ) {
AhrsProgramReceive();
}
#endif
txPacket.status.crcErrors++;
if(linkOk && okCount > 0 ) {
okCount--;
if( okCount == 0 ) {
linkOk = false;
okCount = MIN_OK_FRAMES;
}
}
}
#ifdef IN_AHRS
/*queue next frame
If PIOS_SPI_TransferBlock() fails for any reason, comms will stop working.
In that case, AhrsPoll() should kick start things again.
*/
PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
#endif
}
void PollEvents( void )
{
for( int idx = 0; idx < MAX_AHRS_OBJECTS; idx++ ) {
if( objCallbacks[idx] ) {
PIOS_IRQ_Disable();
if( callbackPending[idx] ) {
callbackPending[idx] = false;
PIOS_IRQ_Enable();
objCallbacks[idx]( AhrsFromIndex( idx ) );
} else {
PIOS_IRQ_Enable();
}
}
}
}
#ifdef IN_AHRS
void AhrsPoll()
{
PollEvents();
if( PIOS_SPI_Busy( PIOS_SPI_OP ) != 0 ) { //Everything is working correctly
return;
}
txPacket.status.kickStarts++;
//comms have broken down - try kick starting it.
txPacket.command = COMMS_NULL; //we must send something so default to null
PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
}
bool AhrsLinkReady( void )
{
for(int ct=0; ct< MAX_AHRS_OBJECTS; ct++)
{
if(!readyObjects[ct])
{
return(false);
}
}
return( linkOk);
}
#else
void AhrsSendObjects( void )
{
static bool oldLink = false;
PollEvents();
if( oldLink != linkOk ) {
oldLink = linkOk;
if( linkOk ) {
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
AhrsObjHandle hdl = AhrsFromIndex( ct );
if( !hdl ) { //paranoid - shouldn't ever happen
continue;
}
AhrsSharedData data;
UAVObjGetData( hdl->uavHandle, &data );
AhrsSetData( hdl, &data );
SetObjectDirty( ct ); //force even unchanged data to be sent
}
}
}
FillObjectPacket();
SendPacket();
}
void SendPacket( void )
{
PIOS_SPI_RC_PinSet( PIOS_OPAHRS_SPI, 0 );
//no point checking if this failed. There isn't much we could do about it if it did fail
PIOS_SPI_TransferBlock( PIOS_OPAHRS_SPI, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
}
static void AhrsUpdatedCb( AhrsObjHandle handle )
{
UAVObjSetData( handle->uavHandle, handle->data );
return;
}
#endif

26
flight/Libraries/ahrs_spi_program_master.c Normal file
View File

@ -0,0 +1,26 @@
#include "ahrs_program_master.h"
#include "ahrs_program.h"
#include "pios_spi.h"
char connectTxBuf[SPI_PROGRAM_REQUEST_LENGTH] = {SPI_PROGRAM_REQUEST};
char connectRxBuf[SPI_PROGRAM_REQUEST_LENGTH];
#define MAX_CONNECT_TRIES 10
uint32_t AhrsProgramConnect( void )
{
memset( connectRxBuf, 0, SPI_PROGRAM_REQUEST_LENGTH );
for( int ct = 0; ct < MAX_CONNECT_TRIES; ct++ ) {
uint32_t res = PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&connectTxBuf,
( uint8_t * )&connectRxBuf, SPI_PROGRAM_REQUEST_LENGTH, NULL );
if( res == 0 && memcmp( connectRxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH ) == 0 ) {
return( 0 );
}
vTaskDelay( 1 / portTICK_RATE_MS );
}
return( -1 );
}

9
flight/Libraries/ahrs_spi_program_slave.c Normal file
View File

@ -0,0 +1,9 @@
#include <stdint.h>
#include "ahrs_spi_program_slave.h"
#include "ahrs_spi_program.h"
//TODO:Implement programming protocol
void AhrsProgramReceive( void )
{
}

67
flight/Libraries/inc/ahrs_comm_objects.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef AHRS_COMM_OBJECTS_H
#define AHRS_COMM_OBJECTS_H
#include "attitudeactual.h"
#include "attituderaw.h"
#include "attitudesettings.h"
#include "ahrsstatus2.h"
#include "baroaltitude.h"
#include "gpsposition.h"
#include "positionactual.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "ahrssettings.h"
/** union that will fit any UAVObject.
*/
typedef union {
AttitudeRawData AttitudeRaw;
AttitudeActualData AttitudeActual;
AHRSCalibrationData AHRSCalibration;
AttitudeSettingsData AttitudeSettings;
AhrsStatus2Data AhrsStatus2;
BaroAltitudeData BaroAltitude;
GPSPositionData GPSPosition;
PositionActualData PositionActual;
HomeLocationData HomeLocation;
AHRSSettingsData AHRSSettings;
} __attribute__(( packed ) ) AhrsSharedData;
/** The number of UAVObjects we will be dealing with.
*/
#define MAX_AHRS_OBJECTS 10
/** Our own version of a UAVObject.
*/
typedef struct {
void * data;
int size;
uint8_t index;
#ifndef IN_AHRS
UAVObjHandle uavHandle;
#endif
} AhrsSharedObject;
typedef AhrsSharedObject * AhrsObjHandle;
/** Initialise the object mapping.
It is important that this is called before any of the following functions.
*/
void AhrsInitHandles( void );
/** the AHRS object related to the given index.
Returns the AHRS object or NULL if not found
*/
AhrsObjHandle AhrsFromIndex( uint8_t index );
#ifndef IN_AHRS
/** Get the AHRS object associated with the UAVObject.
Returns the AHRS object or NULL if not found
*/
AhrsObjHandle AhrsFromUAV( UAVObjHandle obj );
#endif
#endif //#ifndef AHRS_COMMS_OBJECTS_H

111
flight/Libraries/inc/ahrs_spi_comm.h Normal file
View File

@ -0,0 +1,111 @@
#ifndef AHRSCOMMS_H_INCLUDED
#define AHRSCOMMS_H_INCLUDED
#ifdef IN_AHRS //AHRS only
#include <stdint.h>
#include <stdbool.h>
/** Redirect UAVObjGetData call
*/
#define UAVObjGetData(obj, data) AhrsGetData(obj, data)
/** Redirect UAVObjSetData call
*/
#define UAVObjSetData(obj, data) AhrsSetData(obj, data)
/** Redirect UAVObjConnectCallback call
Note: in AHRS, mask is unused because there is only one event type
*/
#define UAVObjConnectCallback(obj, callback, mask) AhrsConnectCallBack(obj,callback)
/** define our own UAVObjHandle
*/
typedef void *UAVObjHandle;
#else
#include "openpilot.h"
#include "uavobjectmanager.h"
#endif
#define AHRS_NO_OBJECT 0xff
#include "ahrs_comm_objects.h"
/** Status of each end of the link
*/
typedef struct { //try to keep this short and in multiples of 4 bytes
uint8_t kickStarts; //AHRS end only
uint8_t crcErrors;
uint8_t retries;
uint8_t invalidPacket;
} AhrsEndStatus;
/** AHRS comms status
*/
typedef struct {
uint8_t linkOk;
AhrsEndStatus remote;
AhrsEndStatus local;
} AhrsCommStatus;
/** Event callback, this function is called when an object changes.
*/
typedef void ( *AhrsEventCallback )( AhrsObjHandle obj );
/** Initialise comms.
Note: this must be called before you do anything else.
Comms will not start until the first call to AhrsPoll() or
AhrsSendObjects()
*/
void AhrsInitComms( void );
/** AHRS version of UAVObject xxxSetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsSetData( AhrsObjHandle obj, const void *dataIn );
/** AHRS version of UAVObject xxxGetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsGetData( AhrsObjHandle obj, void *dataOut );
/** Connect a callback for any changes to AHRS data.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsConnectCallBack( AhrsObjHandle obj, AhrsEventCallback cb );
/** Get the current link status.
Returns: the status.
Note: the remote status will only be valid if the link is up and running
*/
AhrsCommStatus AhrsGetStatus();
#ifdef IN_AHRS //slave only
/** Send the latest objects to the OP
This also polls any pending events and kick starts the DMA
if needed
*/
void AhrsPoll();
/** Check if the link is up and we have received the first batch of updates
Returns: True if the link is up and all objects are up to date
*/
bool AhrsLinkReady();
#else //master only
/** Send the latest objects to the AHRS
This also polls any pending events
*/
void AhrsSendObjects( void );
#endif
#endif //#ifndef AHRSCOMMS_H_INCLUDED

53
flight/Libraries/inc/ahrs_spi_program.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef AHRS_SPI_PROGRAM_H
#define AHRS_SPI_PROGRAM_H
/* Special packets to enter programming mode.
Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long.
Pad with spaces if needed.
*/
#define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING "
#define SPI_PROGRAM_ACK "AHRS PROGRAMMING STARTED"
#define SPI_PROGRAM_REQUEST_LENGTH 24
/**Proposed programming protocol:
In the master:
1) Send a AhrsProgramPacket containing the relevant data.
Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
2) Keep sending PROGRAM_NULL packets and wait for an ack.
Time out if we waited too long.
3) Compare ack packet with transmitted packet. The data
should be the bitwise inverse of the data transmitted.
4) repeat for next packet until finished
5) Repeat using verify packets with data all zeros
Returned data should be exactly as read from memory
In the slave:
1) Wait for an AhrsProgramPacket
2) Check CRC then write to memory
3) Bitwise invert data
4) Transmit packet.
5) repeat until we receive a verify packet
6) verify until we receive a reboot packet
Reboot packets had better have some sort of magic number in the data,
just to be absolutely sure
*/
typedef enum {PROGRAM_NULL, PROGRAM_DATA, PROGRAM_ACK, PROGRAM_VERIFY, PROGRAM_REBOOT} ProgramType;
#define SPI_MAX_PROGRAM_DATA_SIZE 256 //max 256 bytes per packet
/** Proposed program packet defintion
*/
typedef struct {
ProgramType type;
uint32_t address; //base address to place data
uint32_t size; //Size of data
uint8_t data [SPI_MAX_PROGRAM_DATA_SIZE];
uint32_t crc; //CRC32 - hardware CRC8 can be fooled
} AhrsProgramPacket;
#endif

11
flight/Libraries/inc/ahrs_spi_program_master.h Normal file
View File

@ -0,0 +1,11 @@
#ifndef AHRS_PROGRAM_MASTER_H
#define AHRS_PROGRAM_MASTER_H
/** Connect to AHRS and request programming mode
* returns: 0 if connected, -1 if failed.
*/
uint32_t AhrsProgramConnect(void);
//TODO: Implement programming protocol
#endif //AHRS_PROGRAM_MASTER_H

5
flight/Libraries/inc/ahrs_spi_program_slave.h Normal file
View File

@ -0,0 +1,5 @@
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
#define AHRS_SPI_PROGRAM_SLAVE_H
void AhrsProgramReceive(void);
#endif //AHRS_PROGRAM_SLAVE_H

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

@ -0,0 +1,111 @@
/**
******************************************************************************
* @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;
}
/**
* @}
*/

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

@ -0,0 +1,131 @@
/**
******************************************************************************
* @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,6 +34,7 @@
#include "ahrscalibration.h" #include "ahrscalibration.h"
#include "ahrssettings.h" #include "ahrssettings.h"
#include "ahrsstatus.h" #include "ahrsstatus.h"
#include "ahrsstatus2.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudedesired.h" #include "attitudedesired.h"
#include "attituderaw.h" #include "attituderaw.h"
@ -80,6 +81,7 @@ void UAVObjectsInitializeAll()
AHRSCalibrationInitialize(); AHRSCalibrationInitialize();
AHRSSettingsInitialize(); AHRSSettingsInitialize();
AhrsStatusInitialize(); AhrsStatusInitialize();
AhrsStatus2Initialize();
AttitudeActualInitialize(); AttitudeActualInitialize();
AttitudeDesiredInitialize(); AttitudeDesiredInitialize();
AttitudeRawInitialize(); AttitudeRawInitialize();

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

@ -1,97 +1,100 @@
TEMPLATE = lib TEMPLATE = lib
TARGET = UAVObjects TARGET = UAVObjects
DEFINES += UAVOBJECTS_LIBRARY DEFINES += UAVOBJECTS_LIBRARY
include(../../openpilotgcsplugin.pri) include(../../openpilotgcsplugin.pri)
include(uavobjects_dependencies.pri) include(uavobjects_dependencies.pri)
HEADERS += uavobjects_global.h \ HEADERS += uavobjects_global.h \
uavobject.h \ uavobject.h \
uavmetaobject.h \ uavmetaobject.h \
uavobjectmanager.h \ uavobjectmanager.h \
uavdataobject.h \ uavdataobject.h \
uavobjectfield.h \ uavobjectfield.h \
uavobjectsinit.h \ uavobjectsinit.h \
uavobjectsplugin.h \ uavobjectsplugin.h \
examplesettings.h \ examplesettings.h \
ahrsstatus.h \ ahrsstatus.h \
ahrscalibration.h \ ahrscalibration.h \
baroaltitude.h \ baroaltitude.h \
attitudeactual.h \ attitudeactual.h \
ahrssettings.h \ ahrssettings.h \
exampleobject2.h \ exampleobject2.h \
exampleobject1.h \ exampleobject1.h \
gcstelemetrystats.h \ gcstelemetrystats.h \
attituderaw.h \ attituderaw.h \
flighttelemetrystats.h \ flighttelemetrystats.h \
systemstats.h \ systemstats.h \
systemalarms.h \ systemalarms.h \
objectpersistence.h \ objectpersistence.h \
telemetrysettings.h \ telemetrysettings.h \
systemsettings.h \ systemsettings.h \
stabilizationsettings.h \ stabilizationsettings.h \
manualcontrolsettings.h \ manualcontrolsettings.h \
manualcontrolcommand.h \ manualcontrolcommand.h \
attitudedesired.h \ attitudedesired.h \
actuatorsettings.h \ actuatorsettings.h \
actuatordesired.h \ actuatordesired.h \
actuatorcommand.h \ actuatorcommand.h \
gpsposition.h \ gpsposition.h \
gpstime.h \ gpstime.h \
gpssatellites.h \ gpssatellites.h \
positionactual.h \ positionactual.h \
flightbatterystate.h \ flightbatterystate.h \
homelocation.h \ homelocation.h \
vtolsettings.h \ vtolsettings.h \
vtolstatus.h \ vtolstatus.h \
mixersettings.h \ mixersettings.h \
mixerstatus.h \ mixerstatus.h \
velocitydesired.h \ velocitydesired.h \
velocityactual.h \ velocityactual.h \
guidancesettings.h \ guidancesettings.h \
positiondesired.h \ positiondesired.h \
attitudesettings.h attitudesettings.h \
SOURCES += uavobject.cpp \ ahrsstatus2.h
uavmetaobject.cpp \
uavobjectmanager.cpp \ SOURCES += uavobject.cpp \
uavdataobject.cpp \ uavmetaobject.cpp \
uavobjectfield.cpp \ uavobjectmanager.cpp \
uavobjectsinit.cpp \ uavdataobject.cpp \
uavobjectsplugin.cpp \ uavobjectfield.cpp \
ahrsstatus.cpp \ uavobjectsinit.cpp \
ahrscalibration.cpp \ uavobjectsplugin.cpp \
baroaltitude.cpp \ ahrsstatus.cpp \
attitudeactual.cpp \ ahrscalibration.cpp \
ahrssettings.cpp \ baroaltitude.cpp \
examplesettings.cpp \ attitudeactual.cpp \
exampleobject2.cpp \ ahrssettings.cpp \
exampleobject1.cpp \ examplesettings.cpp \
gcstelemetrystats.cpp \ exampleobject2.cpp \
attituderaw.cpp \ exampleobject1.cpp \
flighttelemetrystats.cpp \ gcstelemetrystats.cpp \
systemstats.cpp \ attituderaw.cpp \
systemalarms.cpp \ flighttelemetrystats.cpp \
objectpersistence.cpp \ systemstats.cpp \
telemetrysettings.cpp \ systemalarms.cpp \
systemsettings.cpp \ objectpersistence.cpp \
stabilizationsettings.cpp \ telemetrysettings.cpp \
manualcontrolsettings.cpp \ systemsettings.cpp \
manualcontrolcommand.cpp \ stabilizationsettings.cpp \
attitudedesired.cpp \ manualcontrolsettings.cpp \
actuatorsettings.cpp \ manualcontrolcommand.cpp \
actuatordesired.cpp \ attitudedesired.cpp \
actuatorcommand.cpp \ actuatorsettings.cpp \
gpsposition.cpp \ actuatordesired.cpp \
gpstime.cpp \ actuatorcommand.cpp \
gpssatellites.cpp \ gpsposition.cpp \
positionactual.cpp \ gpstime.cpp \
flightbatterystate.cpp \ gpssatellites.cpp \
homelocation.cpp \ positionactual.cpp \
vtolsettings.cpp \ flightbatterystate.cpp \
vtolstatus.cpp \ homelocation.cpp \
mixersettings.cpp \ vtolsettings.cpp \
mixerstatus.cpp \ vtolstatus.cpp \
velocitydesired.cpp \ mixersettings.cpp \
velocityactual.cpp \ mixerstatus.cpp \
guidancesettings.cpp \ velocitydesired.cpp \
positiondesired.cpp \ velocityactual.cpp \
attitudesettings.cpp guidancesettings.cpp \
OTHER_FILES += UAVObjects.pluginspec positiondesired.cpp \
attitudesettings.cpp \
ahrsstatus2.cpp
OTHER_FILES += UAVObjects.pluginspec

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

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

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

@ -0,0 +1,27 @@
<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>