rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity
0c103f7b2c
LibrePilot/flight/Revolution/ins.c
James Cotton 9bdd021b2e Make new Revolution target
2011-11-01 01:17:59 -05:00

904 lines
26 KiB
C
Raw Blame History

/**
******************************************************************************
* @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 <stdbool.h>
#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
}
/**
* @}
*/
Reference in New Issue View Git Blame Copy Permalink