/** ****************************************************************************** * @addtogroup INS INS * @brief The INS Modules perform * * @{ * @addtogroup INS_Main * @brief Main function which does the hardware dependent stuff * @{ * * * @file ins.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @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 */ /* TODO: BMP085 - Pressure IMU3000 interrupt BMA180 interrupt */ #define TYPICAL_PERIOD 3300 #define timer_rate() 100000 #define timer_count() 1 /* OpenPilot Includes */ #include "ins.h" #include "pios.h" #include "ahrs_spi_comm.h" #include "insgps.h" #include "CoordinateConversions.h" #include "NMEA.h" #include #include "fifo_buffer.h" #include "insgps_helper.h" #define DEG_TO_RAD (M_PI / 180.0) #define RAD_TO_DEG (180.0 / M_PI) #define INSGPS_MAGLEN 1000 #define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */ #define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD))) #define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81))) #define ISNAN(x) (x != x) // down-sampled data index volatile int8_t ahrs_algorithm; /* Data accessors */ void get_gps_data(); void get_mag_data(); void get_baro_data(); void get_accel_gyro_data(); void reset_values(); void measure_noise(void); void zero_gyros(bool update_settings); /* Communication functions */ //void send_calibration(void); void send_attitude(void); void send_velocity(void); void homelocation_callback(AhrsObjHandle obj); //void calibration_callback(AhrsObjHandle obj); void settings_callback(AhrsObjHandle obj); void affine_rotate(float scale[3][4], float rotation[3]); void calibration(float result[3], float scale[3][4], float arg[3]); extern void PIOS_Board_Init(void); void panic(uint32_t blinks); static void print_ekf_binary(bool ekf); void simple_update(); /* Bootloader related functions and var*/ void firmwareiapobj_callback(AhrsObjHandle obj); volatile uint8_t reset_count=0; /** * @addtogroup INS_Global_Data INS Global Data * @{ * Public data. Used by both EKF and the sender */ //! Contains the data from the mag sensor chip struct mag_sensor mag_data; //! Contains the data from the accelerometer struct accel_sensor accel_data; //! Contains the data from the gyro struct gyro_sensor gyro_data; //! Conains the current estimate of the attitude struct attitude_solution attitude_data; //! Contains data from the altitude sensor struct altitude_sensor altitude_data; //! Contains data from the GPS (via the SPI link) struct gps_sensor gps_data; static float mag_len = 0; typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states; /** * @} */ /** * @brief INS Main function */ uint32_t total_conversion_blocks; static bool bias_corrected_raw; float pressure, altitude; int32_t dr; static volatile bool init_algorithm = false; static bool zeroed_gyros = false; int32_t sclk, sclk_prev; int32_t sclk_count; uint32_t loop_time; int main() { gps_data.quality = -1; ahrs_algorithm = INSSETTINGS_ALGORITHM_SIMPLE; reset_values(); PIOS_Board_Init(); PIOS_LED_Off(LED1); PIOS_LED_On(LED2); // Sensors need a second to start PIOS_DELAY_WaitmS(100); #if 0 // Sensor test if(PIOS_IMU3000_Test() != 0) panic(1); if(PIOS_BMA180_Test() != 0) panic(2); if(PIOS_HMC5883_Test() != 0) panic(3); if(PIOS_BMP085_Test() != 0) panic(4); #endif PIOS_LED_On(LED1); PIOS_LED_Off(LED2); // Kickstart BMP085 measurements until driver improved PIOS_BMP085_StartADC(TemperatureConv); // Flash warning light while trying to connect uint32_t time_val1 = PIOS_DELAY_GetRaw(); uint32_t time_val2; uint32_t ms_count = 0; while(!AhrsLinkReady()) { AhrsPoll(); if(PIOS_DELAY_DiffuS(time_val1) > 1000) { ms_count += 1; time_val1 = PIOS_DELAY_GetRaw(); } if(ms_count > 100) { PIOS_LED_Toggle(LED2); ms_count = 0; } } PIOS_LED_Off(LED2); /* we didn't connect the callbacks before because we have to wait for all data to be up to date before doing anything*/ InsSettingsConnectCallback(settings_callback); HomeLocationConnectCallback(homelocation_callback); //FirmwareIAPObjConnectCallback(firmwareiapobj_callback); for(uint32_t i = 0; i < 200; i++) { get_accel_gyro_data(); // This function blocks till data avilable get_mag_data(); get_baro_data(); PIOS_DELAY_WaituS(TYPICAL_PERIOD); } settings_callback(InsSettingsHandle()); ins_init_algorithm(); /******************* Main EKF loop ****************************/ while(1) { AhrsPoll(); InsStatusData status; InsStatusGet(&status); // Alive signal if ((total_conversion_blocks++ % 100) == 0) PIOS_LED_Toggle(LED1); loop_time = PIOS_DELAY_DiffuS(time_val1); time_val1 = PIOS_DELAY_GetRaw(); get_accel_gyro_data(); // This function blocks till data avilable get_mag_data(); get_baro_data(); get_gps_data(); status.IdleTimePerCycle = PIOS_DELAY_DiffuS(time_val1); if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) || ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) || ACCEL_OOB(accel_data.filtered.x) || ACCEL_OOB(accel_data.filtered.y) || ACCEL_OOB(accel_data.filtered.z) || GYRO_OOB(gyro_data.filtered.x) || GYRO_OOB(gyro_data.filtered.y) || GYRO_OOB(gyro_data.filtered.z)) { // If any values are NaN or huge don't update //TODO: add field to ahrs status to track number of these events continue; } if(total_conversion_blocks <= 100 && !zeroed_gyros) { // TODO: Replace this with real init zero_gyros(total_conversion_blocks == 100); if(total_conversion_blocks == 100) zeroed_gyros = true; PIOS_DELAY_WaituS(TYPICAL_PERIOD); float zeros[3] = {0,0,0}; INSSetGyroBias(zeros); continue; } /* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */ if (init_algorithm) { ins_init_algorithm(); init_algorithm = false; } time_val2 = PIOS_DELAY_GetRaw(); print_ekf_binary(true); switch(ahrs_algorithm) { case INSSETTINGS_ALGORITHM_SIMPLE: simple_update(); break; case INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR: ins_outdoor_update(); break; case INSSETTINGS_ALGORITHM_INSGPS_INDOOR: case INSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG: ins_indoor_update(); break; case INSSETTINGS_ALGORITHM_CALIBRATION: measure_noise(); break; case INSSETTINGS_ALGORITHM_SENSORRAW: print_ekf_binary(false); // Run at standard rate while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); break; case INSSETTINGS_ALGORITHM_ZEROGYROS: zero_gyros(false); // Run at standard rate while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); break; } status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2); InsStatusSet(&status); } return 0; } /** * @brief Simple update using just mag and accel. Yaw biased and big attitude changes. */ void simple_update() { 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[1]), (-1 * mag_data.raw.axis[0])) * RAD_TO_DEG; rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * RAD_TO_DEG; rpy[0] = atan2(accel_data.filtered.y, accel_data.filtered.z) * RAD_TO_DEG; 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]; send_attitude(); } /** * @brief Output all the important inputs and states of the ekf through serial port */ static void print_ekf_binary(bool ekf) { static uint32_t timeval; uint16_t delay; delay = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); PIOS_DELAY_WaituS(500); uint8_t framing[] = { 0xff, 0x00, 0xc3, 0x7d }; // Dump raw buffer PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], sizeof(framing)); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.updated, 1); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data.temperature, 4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data.temperature, 4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &delay, 2); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); if(ekf) PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Nav, sizeof(Nav)); // X (86:149) else { mag_data.updated = 0; altitude_data.updated = 0; gps_data.updated = 0; } } void panic(uint32_t blinks) { int blinked = 0; while(1) { PIOS_LED_On(LED2); PIOS_DELAY_WaitmS(200); PIOS_LED_Off(LED2); PIOS_DELAY_WaitmS(200); blinked++; if(blinked >= blinks) { blinked = 0; PIOS_DELAY_WaitmS(1000); } } } /** * @brief Get the accel and gyro data from whichever source when available * * This function will act as the HAL for the new INS sensors */ uint32_t accel_samples; uint32_t gyro_samples; struct pios_bma180_data accel; struct pios_imu3000_data gyro; AttitudeRawData raw; int32_t accel_accum[3] = {0, 0, 0}; int32_t gyro_accum[3] = {0,0,0}; float scaling; void get_accel_gyro_data() { int32_t read_good; int32_t count; for (int i = 0; i < 3; i++) { accel_accum[i] = 0; gyro_accum[i] = 0; } accel_samples = 0; gyro_samples = 0; // Make sure we get one sample count = 0; while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0); while(read_good == 0) { count++; accel_accum[0] += accel.x; accel_accum[1] += accel.y; accel_accum[2] += accel.z; read_good = PIOS_BMA180_ReadFifo(&accel); } accel_samples = count; // Make sure we get one sample count = 0; while((read_good = PIOS_IMU3000_ReadFifo(&gyro)) != 0); while(read_good == 0) { count++; gyro_accum[0] += gyro.x; gyro_accum[1] += gyro.y; gyro_accum[2] += gyro.z; read_good = PIOS_IMU3000_ReadFifo(&gyro); } gyro_samples = count; // Not the swaping of channel orders scaling = PIOS_BMA180_GetScale() / accel_samples; accel_data.filtered.x = accel_accum[0] * scaling * accel_data.calibration.scale[0] + accel_data.calibration.bias[0]; accel_data.filtered.y = -accel_accum[1] * scaling * accel_data.calibration.scale[1] + accel_data.calibration.bias[1]; accel_data.filtered.z = -accel_accum[2] * scaling * accel_data.calibration.scale[2] + accel_data.calibration.bias[2]; scaling = PIOS_IMU3000_GetScale() / gyro_samples; gyro_data.filtered.x = -((float) gyro_accum[1]) * scaling * gyro_data.calibration.scale[0] + gyro_data.calibration.bias[0]; gyro_data.filtered.y = -((float) gyro_accum[0]) * scaling * gyro_data.calibration.scale[1] + gyro_data.calibration.bias[1]; gyro_data.filtered.z = -((float) gyro_accum[2]) * scaling * gyro_data.calibration.scale[2] + gyro_data.calibration.bias[2]; raw.accels[0] = accel_data.filtered.x; raw.accels[1] = accel_data.filtered.y; raw.accels[2] = accel_data.filtered.z; raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG; raw.gyros[1] = gyro_data.filtered.y * RAD_TO_DEG; raw.gyros[2] = gyro_data.filtered.z * RAD_TO_DEG; // From data sheet 35 deg C corresponds to -13200, and 280 LSB per C gyro_data.temperature = 35.0f + ((float) gyro.temperature + 13200) / 280; // From the data sheet 25 deg C corresponds to 2 and 2 LSB per C accel_data.temperature = 25.0f + ((float) accel.temperature - 2) / 2; if (bias_corrected_raw) { raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG; raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG; raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG; raw.accels[0] -= Nav.accel_bias[0]; raw.accels[1] -= Nav.accel_bias[1]; raw.accels[2] -= Nav.accel_bias[2]; } raw.temperature[ATTITUDERAW_TEMPERATURE_GYRO] = gyro_data.temperature; raw.temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = accel_data.temperature; 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 Get the mag data from the I2C sensor and load into structure * @return none * * This function also considers if the home location is set and has a valid * magnetic field before updating the mag data to prevent data being used that * cannot be interpreted. In addition the mag data is not used for the first * five seconds to allow the filter to start to converge */ void get_mag_data() { // Get magnetic readings // For now don't use mags until the magnetic field is set AND until 5 seconds // after initialization otherwise it seems to have problems // TODO: Follow up this initialization issue HomeLocationData home; HomeLocationGet(&home); if (PIOS_HMC5883_NewDataAvailable()) { PIOS_HMC5883_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 = true; } } /** * @brief Get the barometer data * @return none */ uint32_t baro_conversions = 0; void get_baro_data() { int32_t retval = PIOS_BMP085_ReadADC(); if (retval == 0) { // Conversion completed pressure = PIOS_BMP085_GetPressure(); altitude = 44330.0 * (1.0 - powf(pressure / BMP085_P0, (1.0 / 5.255))); BaroAltitudeData data; BaroAltitudeGet(&data); data.Altitude = altitude; data.Pressure = pressure / 1000.0f; data.Temperature = PIOS_BMP085_GetTemperature() / 10.0f; // Convert to deg C BaroAltitudeSet(&data); if((baro_conversions++) % 2) PIOS_BMP085_StartADC(PressureConv); else PIOS_BMP085_StartADC(TemperatureConv); altitude_data.altitude = data.Altitude; altitude_data.updated = true; } } /** * @brief Process any data coming in the gps port */ void get_gps_data() { uint8_t c; static bool start_flag = false; static bool found_cr = false; static char gps_rx_buffer[NMEA_MAX_PACKET_LENGTH]; static uint32_t rx_count = 0; static uint32_t numChecksumErrors = 0; static uint32_t numParsingErrors = 0; static uint32_t numOverflowErrors = 0; static uint32_t numUpdates = 0; while(PIOS_COM_ReceiveBuffer(pios_com_gps_id, &c, 1, 0) == 1) { // Echo data back out aux port //PIOS_COM_SendBufferNonBlocking(pios_com_aux_id, &c, 1); // detect start while acquiring stream if (!start_flag && (c == '$')) { start_flag = true; found_cr = false; rx_count = 0; } else if (!start_flag) continue; if (rx_count >= NMEA_MAX_PACKET_LENGTH) { // The buffer is already full and we haven't found a valid NMEA sentence. // Flush the buffer and note the overflow event. start_flag = false; found_cr = false; rx_count = 0; numOverflowErrors++; } else { gps_rx_buffer[rx_count] = c; rx_count++; } // look for ending '\r\n' sequence if (!found_cr && (c == '\r') ) found_cr = true; else if (found_cr && (c != '\n') ) found_cr = false; // false end flag else if (found_cr && (c == '\n') ) { // The NMEA functions require a zero-terminated string // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n gps_rx_buffer[rx_count-2] = 0; // prepare to parse next sentence start_flag = false; found_cr = false; rx_count = 0; // Our rxBuffer must look like this now: // [0] = '$' // ... = zero or more bytes of sentence payload // [end_pos - 1] = '\r' // [end_pos] = '\n' // // Prepare to consume the sentence from the buffer // Validate the checksum over the sentence if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx. //PIOS_DEBUG_PinHigh(2); ++numChecksumErrors; //PIOS_DEBUG_PinLow(2); } else { // Valid checksum, use this packet to update the GPS position if (!NMEA_update_position(&gps_rx_buffer[1])) { //PIOS_DEBUG_PinHigh(2); ++numParsingErrors; //PIOS_DEBUG_PinLow(2); } else { ++numUpdates; GPSPositionData pos; GPSPositionGet(&pos); HomeLocationData home; HomeLocationGet(&home); // convert from cm back to meters double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)}; // put in local NED frame 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 = pos.Satellites; gps_data.updated = true; const uint32_t INSGPS_GPS_MINSAT = 6; const float INSGPS_GPS_MINPDOP = 4; // if poor don't use this update if((ahrs_algorithm != INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (pos.Satellites < INSGPS_GPS_MINSAT) || (pos.PDOP >= INSGPS_GPS_MINPDOP) || (home.Set == HOMELOCATION_SET_FALSE) || ((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0))) { gps_data.updated = false; } } } } } } /** * @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 NMEAN 500 #define NVAR 1000 #define CHANNELS 6 static uint32_t calibrate_count = 0; float f_means[CHANNELS]; float f_var[CHANNELS] = {0, 0, 0, 0, 0, 0}; void measure_noise() { uint32_t i; float data[CHANNELS] = {accel_data.filtered.x, accel_data.filtered.y, accel_data.filtered.z, gyro_data.filtered.x, gyro_data.filtered.y, gyro_data.filtered.z }; // First step, zero all sufficient statistics if(calibrate_count == 0) { for (i = 0; i < CHANNELS; i++) { f_means[i] = 0; f_var[i] = 0; } } // Accumulate for an estimate of mean if(calibrate_count < NMEAN) for (i = 0; i < CHANNELS; i++) f_means[i] += data[i]; if(calibrate_count == NMEAN) for (i = 0; i < CHANNELS; i++) f_means[i] /= (float) NMEAN; // Accumulate for estimate of variance. This needs to be done // sequentially because storing second moment would go out of // float precision if(calibrate_count >= NMEAN && calibrate_count < (NMEAN + NVAR)) for (i = 0; i < CHANNELS; i++) f_var[i] += pow(f_means[i] - data[i],2); if(calibrate_count == (NMEAN + NVAR)) { for (i = 0; i < CHANNELS; i++) f_var[i] /= (float) (NVAR - 1); calibrate_count = 0; InsSettingsData settings; InsSettingsGet(&settings); settings.Algorithm = INSSETTINGS_ALGORITHM_NONE; settings.accel_var[0] = f_var[0]; settings.accel_var[1] = f_var[1]; settings.accel_var[2] = f_var[2]; settings.gyro_var[0] = f_var[3]; settings.gyro_var[1] = f_var[4]; settings.gyro_var[2] = f_var[5]; InsSettingsSet(&settings); settings_callback(InsSettingsHandle()); } else { PIOS_DELAY_WaituS(TYPICAL_PERIOD); calibrate_count++; } } void zero_gyros(bool update_settings) { const float rate = 1e-2; gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate; gyro_data.calibration.bias[1] += -gyro_data.filtered.y * rate; gyro_data.calibration.bias[2] += -gyro_data.filtered.z * rate; if(update_settings) { InsSettingsData settings; InsSettingsGet(&settings); settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X] = gyro_data.calibration.bias[0]; settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y] = gyro_data.calibration.bias[1]; settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z] = gyro_data.calibration.bias[2]; InsSettingsSet(&settings); } } /** * @brief Populate fields with initial values */ void reset_values() { accel_data.calibration.scale[0] = 1; accel_data.calibration.scale[1] = 1; accel_data.calibration.scale[2] = 1; accel_data.calibration.bias[0] = 0; accel_data.calibration.bias[1] = 0; accel_data.calibration.bias[2] = 0; accel_data.calibration.variance[0] = 1; accel_data.calibration.variance[1] = 1; accel_data.calibration.variance[2] = 1; gyro_data.calibration.scale[0] = 1; gyro_data.calibration.scale[1] = 1; gyro_data.calibration.scale[2] = 1; gyro_data.calibration.bias[0] = 0; gyro_data.calibration.bias[1] = 0; gyro_data.calibration.bias[2] = 0; 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] = 50; mag_data.calibration.variance[1] = 50; mag_data.calibration.variance[2] = 50; ahrs_algorithm = INSSETTINGS_ALGORITHM_NONE; } void send_attitude(void) { AttitudeActualData attitude; AttitudeActualGet(&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_velocity(void) { VelocityActualData velocityActual; VelocityActualGet(&velocityActual); // convert into cm velocityActual.North = Nav.Vel[0] * 100; velocityActual.East = Nav.Vel[1] * 100; velocityActual.Down = Nav.Vel[2] * 100; VelocityActualSet(&velocityActual); } int callback_count = 0; void settings_callback(AhrsObjHandle obj) { callback_count ++; InsSettingsData settings; InsSettingsGet(&settings); init_algorithm = ahrs_algorithm != settings.Algorithm; ahrs_algorithm = settings.Algorithm; bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE; accel_data.calibration.scale[0] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_X]; accel_data.calibration.scale[1] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Y]; accel_data.calibration.scale[2] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Z]; accel_data.calibration.bias[0] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_X]; accel_data.calibration.bias[1] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Y]; accel_data.calibration.bias[2] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Z]; accel_data.calibration.variance[0] = settings.accel_var[INSSETTINGS_ACCEL_VAR_X]; accel_data.calibration.variance[1] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Y]; accel_data.calibration.variance[2] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Z]; gyro_data.calibration.scale[0] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_X]; gyro_data.calibration.scale[1] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Y]; gyro_data.calibration.scale[2] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Z]; gyro_data.calibration.bias[0] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X]; gyro_data.calibration.bias[1] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y]; gyro_data.calibration.bias[2] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z]; gyro_data.calibration.variance[0] = settings.gyro_var[INSSETTINGS_GYRO_VAR_X]; gyro_data.calibration.variance[1] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Y]; gyro_data.calibration.variance[2] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Z]; mag_data.calibration.scale[0] = settings.mag_scale[INSSETTINGS_MAG_SCALE_X]; mag_data.calibration.scale[1] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Y]; mag_data.calibration.scale[2] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Z]; mag_data.calibration.bias[0] = settings.mag_bias[INSSETTINGS_MAG_BIAS_X]; mag_data.calibration.bias[1] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Y]; mag_data.calibration.bias[2] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Z]; mag_data.calibration.variance[0] = settings.mag_var[INSSETTINGS_MAG_VAR_X]; mag_data.calibration.variance[1] = settings.mag_var[INSSETTINGS_MAG_VAR_Y]; mag_data.calibration.variance[2] = settings.mag_var[INSSETTINGS_MAG_VAR_Z]; } void homelocation_callback(AhrsObjHandle obj) { HomeLocationData data; HomeLocationGet(&data); mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2)); float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len}; INSSetMagNorth(Be); init_algorithm = true; } void firmwareiapobj_callback(AhrsObjHandle obj) { #if 0 const struct pios_board_info * bdinfo = &pios_board_info_blob; FirmwareIAPObjData firmwareIAPObj; FirmwareIAPObjGet(&firmwareIAPObj); if(firmwareIAPObj.ArmReset==0) reset_count=0; if(firmwareIAPObj.ArmReset==1) { if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF)) { ++reset_count; if(reset_count>2) { PIOS_IAP_SetRequest1(); PIOS_IAP_SetRequest2(); PIOS_SYS_Reset(); } } } else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc()) { PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size); firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc(); firmwareIAPObj.BoardRevision=bdinfo->board_rev; FirmwareIAPObjSet(&firmwareIAPObj); } #endif } /** * @} */