rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Clean up main INS function a bit.

Browse Source
This commit is contained in:
James Cotton 2011-08-19 14:34:56 -05:00
parent d103541970
commit cefc2b8c2e
2 changed files with 23 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/INS/Makefile
View File

@ -35,7 +35,7 @@ OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= YES
DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES

39
flight/INS/ins.c
View File

@ -159,7 +159,7 @@ int32_t dr;
int32_t sclk, sclk_prev;
int32_t sclk_count;
uint32_t loop_time;
int main()
{
gps_data.quality = -1;
@ -175,6 +175,7 @@ int main()
// Sensors need a second to start
PIOS_DELAY_WaitmS(100);
// Sensor test
if(PIOS_IMU3000_Test() != 0)
panic(1);
@ -225,11 +226,14 @@ int main()
AhrsStatusSet(&status);
// Alive signal
if ((total_conversion_blocks % 300) == 0)
if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
total_conversion_blocks++;
// Delay for valid data
loop_time = PIOS_DELAY_DiffuS(time_val);
time_val = PIOS_DELAY_GetRaw();
// This function blocks till data avilable
get_accel_gyro_data();
@ -347,10 +351,9 @@ static void panic(uint32_t blinks)
*
* This function will act as the HAL for the new INS sensors
*/
uint16_t accel_samples = 0;
uint8_t gyro_len;
int32_t gyro_accum[3];
int16_t gyro_fifo[4];
uint8_t accel_samples;
uint8_t gyro_samples;
bool get_accel_gyro_data()
{
int16_t accel[3];
@ -370,26 +373,23 @@ bool get_accel_gyro_data()
accel[1] = accel_accum[1] / accel_samples;
accel[2] = accel_accum[2] / accel_samples;
gyro_accum[0] = 0;
gyro_accum[1] = 0;
gyro_accum[2] = 0;
int32_t gyro_accum[3] = {0,0,0};
int16_t gyro[3];
gyro_len = 0;
int32_t read_good;
// Make sure we get one sample
while((read_good = PIOS_IMU3000_ReadFifo(gyro_fifo)) != 0);
while((read_good = PIOS_IMU3000_ReadFifo(gyro)) != 0);
while(read_good == 0) {
gyro_len++;
gyro_samples++;
gyro_accum[0] += gyro_fifo[0];
gyro_accum[1] += gyro_fifo[1];
gyro_accum[2] += gyro_fifo[2];
read_good = PIOS_IMU3000_ReadFifo(gyro_fifo);
gyro_accum[0] += gyro[0];
gyro_accum[1] += gyro[1];
gyro_accum[2] += gyro[2];
read_good = PIOS_IMU3000_ReadFifo(gyro);
}
gyro[0] = gyro_accum[0] / gyro_len;
gyro[1] = gyro_accum[1] / gyro_len;
gyro[2] = gyro_accum[2] / gyro_len;
gyro[0] = gyro_accum[0] / gyro_samples;
gyro[1] = gyro_accum[1] / gyro_samples;
gyro[2] = gyro_accum[2] / gyro_samples;
// Not the swaping of channel orders
accel_data.filtered.x = accel[0] * PIOS_BMA180_GetScale();
@ -399,7 +399,6 @@ bool get_accel_gyro_data()
gyro_data.filtered.y = -gyro[0] * 0.00763 * DEG_TO_RAD;;
gyro_data.filtered.z = -gyro[2] * 0.00763 * DEG_TO_RAD;;
AttitudeRawData raw;
raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG;