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 developer code and compile options
# Set to YES for debugging # Set to YES for debugging
DEBUG ?= YES DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain # Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES 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, sclk_prev;
int32_t sclk_count; int32_t sclk_count;
uint32_t loop_time;
int main() int main()
{ {
gps_data.quality = -1; gps_data.quality = -1;
@ -175,6 +175,7 @@ int main()
// Sensors need a second to start // Sensors need a second to start
PIOS_DELAY_WaitmS(100); PIOS_DELAY_WaitmS(100);
// Sensor test // Sensor test
if(PIOS_IMU3000_Test() != 0) if(PIOS_IMU3000_Test() != 0)
panic(1); panic(1);
@ -225,11 +226,14 @@ int main()
AhrsStatusSet(&status); AhrsStatusSet(&status);
// Alive signal // Alive signal
if ((total_conversion_blocks % 300) == 0) if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED1);
total_conversion_blocks++; total_conversion_blocks++;
// Delay for valid data // Delay for valid data
loop_time = PIOS_DELAY_DiffuS(time_val);
time_val = PIOS_DELAY_GetRaw();
// This function blocks till data avilable // This function blocks till data avilable
get_accel_gyro_data(); 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 * This function will act as the HAL for the new INS sensors
*/ */
uint16_t accel_samples = 0;
uint8_t gyro_len; uint8_t accel_samples;
int32_t gyro_accum[3]; uint8_t gyro_samples;
int16_t gyro_fifo[4];
bool get_accel_gyro_data() bool get_accel_gyro_data()
{ {
int16_t accel[3]; int16_t accel[3];
@ -370,26 +373,23 @@ bool get_accel_gyro_data()
accel[1] = accel_accum[1] / accel_samples; accel[1] = accel_accum[1] / accel_samples;
accel[2] = accel_accum[2] / accel_samples; accel[2] = accel_accum[2] / accel_samples;
gyro_accum[0] = 0; int32_t gyro_accum[3] = {0,0,0};
gyro_accum[1] = 0;
gyro_accum[2] = 0;
int16_t gyro[3]; int16_t gyro[3];
gyro_len = 0;
int32_t read_good; int32_t read_good;
// Make sure we get one sample // 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) { while(read_good == 0) {
gyro_len++; gyro_samples++;
gyro_accum[0] += gyro_fifo[0]; gyro_accum[0] += gyro[0];
gyro_accum[1] += gyro_fifo[1]; gyro_accum[1] += gyro[1];
gyro_accum[2] += gyro_fifo[2]; gyro_accum[2] += gyro[2];
read_good = PIOS_IMU3000_ReadFifo(gyro_fifo); read_good = PIOS_IMU3000_ReadFifo(gyro);
} }
gyro[0] = gyro_accum[0] / gyro_len; gyro[0] = gyro_accum[0] / gyro_samples;
gyro[1] = gyro_accum[1] / gyro_len; gyro[1] = gyro_accum[1] / gyro_samples;
gyro[2] = gyro_accum[2] / gyro_len; gyro[2] = gyro_accum[2] / gyro_samples;
// Not the swaping of channel orders // Not the swaping of channel orders
accel_data.filtered.x = accel[0] * PIOS_BMA180_GetScale(); 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.y = -gyro[0] * 0.00763 * DEG_TO_RAD;;
gyro_data.filtered.z = -gyro[2] * 0.00763 * DEG_TO_RAD;; gyro_data.filtered.z = -gyro[2] * 0.00763 * DEG_TO_RAD;;
AttitudeRawData raw; AttitudeRawData raw;
raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG; raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG;