From b2d56ca34c803ede5907f54cf7613612000ad407 Mon Sep 17 00:00:00 2001 From: les Date: Fri, 1 Oct 2010 17:31:13 +0000 Subject: [PATCH] AHRS: Added UAVObject based SPI communication between AHRS and OP. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1830 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs_newspi.c | 858 ++++++++++++++++++ flight/Libraries/ahrs_comm_objects.c | 138 +++ flight/Libraries/ahrs_spi_comm.c | 421 +++++++++ flight/Libraries/ahrs_spi_program_master.c | 26 + flight/Libraries/ahrs_spi_program_slave.c | 9 + flight/Libraries/inc/ahrs_comm_objects.h | 67 ++ flight/Libraries/inc/ahrs_spi_comm.h | 111 +++ flight/Libraries/inc/ahrs_spi_program.h | 53 ++ .../Libraries/inc/ahrs_spi_program_master.h | 11 + flight/Libraries/inc/ahrs_spi_program_slave.h | 5 + flight/OpenPilot/UAVObjects/ahrsstatus2.c | 111 +++ flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h | 131 +++ flight/OpenPilot/UAVObjects/uavobjectsinit.c | 2 + ground/src/plugins/uavobjects/uavobjects.pro | 197 ++-- .../src/plugins/uavobjects/uavobjectsinit.cpp | 2 + .../uavobjectdefinition/ahrsstatus2.xml | 27 + 16 files changed, 2072 insertions(+), 97 deletions(-) create mode 100644 flight/AHRS/ahrs_newspi.c create mode 100644 flight/Libraries/ahrs_comm_objects.c create mode 100644 flight/Libraries/ahrs_spi_comm.c create mode 100644 flight/Libraries/ahrs_spi_program_master.c create mode 100644 flight/Libraries/ahrs_spi_program_slave.c create mode 100644 flight/Libraries/inc/ahrs_comm_objects.h create mode 100644 flight/Libraries/inc/ahrs_spi_comm.h create mode 100644 flight/Libraries/inc/ahrs_spi_program.h create mode 100644 flight/Libraries/inc/ahrs_spi_program_master.h create mode 100644 flight/Libraries/inc/ahrs_spi_program_slave.h create mode 100644 flight/OpenPilot/UAVObjects/ahrsstatus2.c create mode 100644 flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h create mode 100644 ground/src/shared/uavobjectdefinition/ahrsstatus2.xml diff --git a/flight/AHRS/ahrs_newspi.c b/flight/AHRS/ahrs_newspi.c new file mode 100644 index 000000000..f87776af6 --- /dev/null +++ b/flight/AHRS/ahrs_newspi.c @@ -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; + } +} + + + +/** + * @} + */ diff --git a/flight/Libraries/ahrs_comm_objects.c b/flight/Libraries/ahrs_comm_objects.c new file mode 100644 index 000000000..05d7feceb --- /dev/null +++ b/flight/Libraries/ahrs_comm_objects.c @@ -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 + diff --git a/flight/Libraries/ahrs_spi_comm.c b/flight/Libraries/ahrs_spi_comm.c new file mode 100644 index 000000000..fa7d4d9d4 --- /dev/null +++ b/flight/Libraries/ahrs_spi_comm.c @@ -0,0 +1,421 @@ +#include "ahrs_spi_comm.h" +#include "ahrs_spi_program.h" + +#ifdef IN_AHRS +#include +#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 diff --git a/flight/Libraries/ahrs_spi_program_master.c b/flight/Libraries/ahrs_spi_program_master.c new file mode 100644 index 000000000..4ad605f4a --- /dev/null +++ b/flight/Libraries/ahrs_spi_program_master.c @@ -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 ); +} + diff --git a/flight/Libraries/ahrs_spi_program_slave.c b/flight/Libraries/ahrs_spi_program_slave.c new file mode 100644 index 000000000..276ef43f7 --- /dev/null +++ b/flight/Libraries/ahrs_spi_program_slave.c @@ -0,0 +1,9 @@ +#include +#include "ahrs_spi_program_slave.h" +#include "ahrs_spi_program.h" + +//TODO:Implement programming protocol +void AhrsProgramReceive( void ) +{ + +} diff --git a/flight/Libraries/inc/ahrs_comm_objects.h b/flight/Libraries/inc/ahrs_comm_objects.h new file mode 100644 index 000000000..cc361b4d0 --- /dev/null +++ b/flight/Libraries/inc/ahrs_comm_objects.h @@ -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 diff --git a/flight/Libraries/inc/ahrs_spi_comm.h b/flight/Libraries/inc/ahrs_spi_comm.h new file mode 100644 index 000000000..7f79c11d5 --- /dev/null +++ b/flight/Libraries/inc/ahrs_spi_comm.h @@ -0,0 +1,111 @@ +#ifndef AHRSCOMMS_H_INCLUDED +#define AHRSCOMMS_H_INCLUDED + + +#ifdef IN_AHRS //AHRS only +#include +#include + +/** 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 diff --git a/flight/Libraries/inc/ahrs_spi_program.h b/flight/Libraries/inc/ahrs_spi_program.h new file mode 100644 index 000000000..12d2fa025 --- /dev/null +++ b/flight/Libraries/inc/ahrs_spi_program.h @@ -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 diff --git a/flight/Libraries/inc/ahrs_spi_program_master.h b/flight/Libraries/inc/ahrs_spi_program_master.h new file mode 100644 index 000000000..b7d3fa1e0 --- /dev/null +++ b/flight/Libraries/inc/ahrs_spi_program_master.h @@ -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 diff --git a/flight/Libraries/inc/ahrs_spi_program_slave.h b/flight/Libraries/inc/ahrs_spi_program_slave.h new file mode 100644 index 000000000..7b3938af2 --- /dev/null +++ b/flight/Libraries/inc/ahrs_spi_program_slave.h @@ -0,0 +1,5 @@ +#ifndef AHRS_SPI_PROGRAM_SLAVE_H +#define AHRS_SPI_PROGRAM_SLAVE_H + +void AhrsProgramReceive(void); +#endif //AHRS_PROGRAM_SLAVE_H diff --git a/flight/OpenPilot/UAVObjects/ahrsstatus2.c b/flight/OpenPilot/UAVObjects/ahrsstatus2.c new file mode 100644 index 000000000..f058058d1 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/ahrsstatus2.c @@ -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; +} + +/** + * @} + */ + diff --git a/flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h b/flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h new file mode 100644 index 000000000..c862049b4 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/inc/ahrsstatus2.h @@ -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 + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/UAVObjects/uavobjectsinit.c b/flight/OpenPilot/UAVObjects/uavobjectsinit.c index 168dbe00d..b1a2d3bb8 100644 --- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c +++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c @@ -34,6 +34,7 @@ #include "ahrscalibration.h" #include "ahrssettings.h" #include "ahrsstatus.h" +#include "ahrsstatus2.h" #include "attitudeactual.h" #include "attitudedesired.h" #include "attituderaw.h" @@ -80,6 +81,7 @@ void UAVObjectsInitializeAll() AHRSCalibrationInitialize(); AHRSSettingsInitialize(); AhrsStatusInitialize(); + AhrsStatus2Initialize(); AttitudeActualInitialize(); AttitudeDesiredInitialize(); AttitudeRawInitialize(); diff --git a/ground/src/plugins/uavobjects/uavobjects.pro b/ground/src/plugins/uavobjects/uavobjects.pro index 859f3a25f..998f9c5fe 100644 --- a/ground/src/plugins/uavobjects/uavobjects.pro +++ b/ground/src/plugins/uavobjects/uavobjects.pro @@ -1,97 +1,100 @@ -TEMPLATE = lib -TARGET = UAVObjects -DEFINES += UAVOBJECTS_LIBRARY -include(../../openpilotgcsplugin.pri) -include(uavobjects_dependencies.pri) -HEADERS += uavobjects_global.h \ - uavobject.h \ - uavmetaobject.h \ - uavobjectmanager.h \ - uavdataobject.h \ - uavobjectfield.h \ - uavobjectsinit.h \ - uavobjectsplugin.h \ - examplesettings.h \ - ahrsstatus.h \ - ahrscalibration.h \ - baroaltitude.h \ - attitudeactual.h \ - ahrssettings.h \ - exampleobject2.h \ - exampleobject1.h \ - gcstelemetrystats.h \ - attituderaw.h \ - flighttelemetrystats.h \ - systemstats.h \ - systemalarms.h \ - objectpersistence.h \ - telemetrysettings.h \ - systemsettings.h \ - stabilizationsettings.h \ - manualcontrolsettings.h \ - manualcontrolcommand.h \ - attitudedesired.h \ - actuatorsettings.h \ - actuatordesired.h \ - actuatorcommand.h \ - gpsposition.h \ - gpstime.h \ - gpssatellites.h \ - positionactual.h \ - flightbatterystate.h \ - homelocation.h \ - vtolsettings.h \ - vtolstatus.h \ - mixersettings.h \ - mixerstatus.h \ - velocitydesired.h \ - velocityactual.h \ - guidancesettings.h \ - positiondesired.h \ - attitudesettings.h -SOURCES += uavobject.cpp \ - uavmetaobject.cpp \ - uavobjectmanager.cpp \ - uavdataobject.cpp \ - uavobjectfield.cpp \ - uavobjectsinit.cpp \ - uavobjectsplugin.cpp \ - ahrsstatus.cpp \ - ahrscalibration.cpp \ - baroaltitude.cpp \ - attitudeactual.cpp \ - ahrssettings.cpp \ - examplesettings.cpp \ - exampleobject2.cpp \ - exampleobject1.cpp \ - gcstelemetrystats.cpp \ - attituderaw.cpp \ - flighttelemetrystats.cpp \ - systemstats.cpp \ - systemalarms.cpp \ - objectpersistence.cpp \ - telemetrysettings.cpp \ - systemsettings.cpp \ - stabilizationsettings.cpp \ - manualcontrolsettings.cpp \ - manualcontrolcommand.cpp \ - attitudedesired.cpp \ - actuatorsettings.cpp \ - actuatordesired.cpp \ - actuatorcommand.cpp \ - gpsposition.cpp \ - gpstime.cpp \ - gpssatellites.cpp \ - positionactual.cpp \ - flightbatterystate.cpp \ - homelocation.cpp \ - vtolsettings.cpp \ - vtolstatus.cpp \ - mixersettings.cpp \ - mixerstatus.cpp \ - velocitydesired.cpp \ - velocityactual.cpp \ - guidancesettings.cpp \ - positiondesired.cpp \ - attitudesettings.cpp -OTHER_FILES += UAVObjects.pluginspec +TEMPLATE = lib +TARGET = UAVObjects +DEFINES += UAVOBJECTS_LIBRARY +include(../../openpilotgcsplugin.pri) +include(uavobjects_dependencies.pri) +HEADERS += uavobjects_global.h \ + uavobject.h \ + uavmetaobject.h \ + uavobjectmanager.h \ + uavdataobject.h \ + uavobjectfield.h \ + uavobjectsinit.h \ + uavobjectsplugin.h \ + examplesettings.h \ + ahrsstatus.h \ + ahrscalibration.h \ + baroaltitude.h \ + attitudeactual.h \ + ahrssettings.h \ + exampleobject2.h \ + exampleobject1.h \ + gcstelemetrystats.h \ + attituderaw.h \ + flighttelemetrystats.h \ + systemstats.h \ + systemalarms.h \ + objectpersistence.h \ + telemetrysettings.h \ + systemsettings.h \ + stabilizationsettings.h \ + manualcontrolsettings.h \ + manualcontrolcommand.h \ + attitudedesired.h \ + actuatorsettings.h \ + actuatordesired.h \ + actuatorcommand.h \ + gpsposition.h \ + gpstime.h \ + gpssatellites.h \ + positionactual.h \ + flightbatterystate.h \ + homelocation.h \ + vtolsettings.h \ + vtolstatus.h \ + mixersettings.h \ + mixerstatus.h \ + velocitydesired.h \ + velocityactual.h \ + guidancesettings.h \ + positiondesired.h \ + attitudesettings.h \ + ahrsstatus2.h + +SOURCES += uavobject.cpp \ + uavmetaobject.cpp \ + uavobjectmanager.cpp \ + uavdataobject.cpp \ + uavobjectfield.cpp \ + uavobjectsinit.cpp \ + uavobjectsplugin.cpp \ + ahrsstatus.cpp \ + ahrscalibration.cpp \ + baroaltitude.cpp \ + attitudeactual.cpp \ + ahrssettings.cpp \ + examplesettings.cpp \ + exampleobject2.cpp \ + exampleobject1.cpp \ + gcstelemetrystats.cpp \ + attituderaw.cpp \ + flighttelemetrystats.cpp \ + systemstats.cpp \ + systemalarms.cpp \ + objectpersistence.cpp \ + telemetrysettings.cpp \ + systemsettings.cpp \ + stabilizationsettings.cpp \ + manualcontrolsettings.cpp \ + manualcontrolcommand.cpp \ + attitudedesired.cpp \ + actuatorsettings.cpp \ + actuatordesired.cpp \ + actuatorcommand.cpp \ + gpsposition.cpp \ + gpstime.cpp \ + gpssatellites.cpp \ + positionactual.cpp \ + flightbatterystate.cpp \ + homelocation.cpp \ + vtolsettings.cpp \ + vtolstatus.cpp \ + mixersettings.cpp \ + mixerstatus.cpp \ + velocitydesired.cpp \ + velocityactual.cpp \ + guidancesettings.cpp \ + positiondesired.cpp \ + attitudesettings.cpp \ + ahrsstatus2.cpp +OTHER_FILES += UAVObjects.pluginspec diff --git a/ground/src/plugins/uavobjects/uavobjectsinit.cpp b/ground/src/plugins/uavobjects/uavobjectsinit.cpp index 893e17432..323aede8a 100644 --- a/ground/src/plugins/uavobjects/uavobjectsinit.cpp +++ b/ground/src/plugins/uavobjects/uavobjectsinit.cpp @@ -36,6 +36,7 @@ #include "ahrscalibration.h" #include "ahrssettings.h" #include "ahrsstatus.h" +#include "ahrsstatus2.h" #include "attitudeactual.h" #include "attitudedesired.h" #include "attituderaw.h" @@ -82,6 +83,7 @@ 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() ); diff --git a/ground/src/shared/uavobjectdefinition/ahrsstatus2.xml b/ground/src/shared/uavobjectdefinition/ahrsstatus2.xml new file mode 100644 index 000000000..d5fcda189 --- /dev/null +++ b/ground/src/shared/uavobjectdefinition/ahrsstatus2.xml @@ -0,0 +1,27 @@ + + + Status for the @ref AHRSCommsModule, including communication errors + + + + + + + + + + + + + + + + + + + + + + + +