rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

OP-140 AHRS: Don't run INSGPS until OP says board is initialized. This will

Browse Source 
also block heart beat.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1767 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-26 03:06:57 +00:00 committed by peabody124
parent 8475a68958
commit 6d811cc4db
1 changed files with 5 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/AHRS/ahrs.c
View File

@ -155,7 +155,6 @@ struct gps_sensor {
void process_spi_request(void); void process_spi_request(void);
void downsample_data(void); void downsample_data(void);
void calibrate_sensors(void); void calibrate_sensors(void);
void converge_insgps();
void reset_values(); void reset_values();
volatile uint32_t last_counter_idle_start = 0; volatile uint32_t last_counter_idle_start = 0;
@ -234,11 +233,11 @@ int main()
fir_coeffs[i] = 1; fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling; fir_coeffs[adc_oversampling] = adc_oversampling;
if (ahrs_algorithm == INSGPS_Algo) { INSGPSInit();
// compute a data point and initialize INS
downsample_data(); while(initialized != AHRS_INITIALIZED)
converge_insgps(); process_spi_request();
}
#ifdef DUMP_RAW #ifdef DUMP_RAW
int previous_conversion; int previous_conversion;
while (1) { while (1) {
@ -644,21 +643,6 @@ void calibrate_sensors()
mag_data.calibration.variance[2] = mag_data.calibration.variance[2] / j - pow(mag_data.calibration.bias[2],2); mag_data.calibration.variance[2] = mag_data.calibration.variance[2] / j - pow(mag_data.calibration.bias[2],2);
} }
/**
* @brief Quickly initialize INS assuming stationary and gravity is down
*
* Currently this is done iteratively but I'm sure it can be directly computed
* when I sit down and work it out
*/
void converge_insgps()
{
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]}; // order swap
INSGPSInit();
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
INSSetMagVar(mag_var);
}
/** /**
* @brief Populate fields with initial values * @brief Populate fields with initial values
*/ */