diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 6ab18a190..4f94b3821 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -114,7 +114,6 @@ 6581785414A65B9B0007885F /* pios_bl_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bl_helper.c; sourceTree = ""; }; 6589A9DB131DEE76006BD67C /* pios_rtc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rtc.c; sourceTree = ""; }; 6589A9E2131DF1C7006BD67C /* pios_rtc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc.h; sourceTree = ""; }; - 65904E47146128A500FD9482 /* ins.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ins.c; sourceTree = ""; }; 65904E4B146128A500FD9482 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = ""; }; 65904E4D146128A500FD9482 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = alarms.c; sourceTree = ""; }; 65904E4E146128A500FD9482 /* cm3_fault_handlers.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = cm3_fault_handlers.c; sourceTree = ""; }; @@ -3816,7 +3815,6 @@ 65904E3F146128A500FD9482 /* Revolution */ = { isa = PBXGroup; children = ( - 65904E47146128A500FD9482 /* ins.c */, 65904E4B146128A500FD9482 /* Makefile */, 65904E4C146128A500FD9482 /* System */, 65904E5B146128A500FD9482 /* test.c */, diff --git a/flight/Revolution/ins.c b/flight/Revolution/ins.c deleted file mode 100644 index 2f47d8519..000000000 --- a/flight/Revolution/ins.c +++ /dev/null @@ -1,903 +0,0 @@ -/** - ****************************************************************************** - * @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() -{ -// *(volatile unsigned long *)0xE000ED88 |= (0xf << 20); - - PIOS_Board_Init(); - PIOS_LED_Off(LED1); - PIOS_LED_On(LED2); - - // Sensors need a second to start - PIOS_DELAY_WaitmS(100); - - ahrs_algorithm = INSSETTINGS_ALGORITHM_SIMPLE; - reset_values(); - gps_data.quality = -1; -#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 -} - - -/** - * @} - */ -