mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
AHRS: Gets timing working correctly to compute the EKF rate and idle time in real values
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1615 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
0875dc8442
commit
c5fe985a45
@ -91,6 +91,7 @@ SRC = ahrs.c
|
||||
SRC += pios_board.c
|
||||
SRC += ahrs_adc.c
|
||||
SRC += ahrs_fsm.c
|
||||
SRC += ahrs_timer.c
|
||||
SRC += insgps.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
/* OpenPilot Includes */
|
||||
#include "ahrs.h"
|
||||
#include "ahrs_adc.h"
|
||||
#include "ahrs_timer.h"
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "ahrs_fsm.h" /* lfsm_state */
|
||||
#include "insgps.h"
|
||||
@ -151,8 +152,12 @@ void process_spi_request(void);
|
||||
void downsample_data(void);
|
||||
void calibrate_sensors(void);
|
||||
void converge_insgps();
|
||||
void timer_start();
|
||||
uint32_t timer_count();
|
||||
|
||||
volatile uint32_t last_counter_idle_start = 0;
|
||||
volatile uint32_t last_counter_idle_end = 0;
|
||||
volatile uint32_t idle_counts;
|
||||
volatile uint32_t running_counts;
|
||||
uint32_t counter_val;
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Global_Data AHRS Global Data
|
||||
@ -183,10 +188,8 @@ double BaseECEF[3] = {0, 0, 0};
|
||||
float Rne[3][3];
|
||||
//! Indicates the communications are requesting a calibration
|
||||
uint8_t calibration_pending = FALSE;
|
||||
//! Counter for tracking the idle level
|
||||
static uint32_t idle_counter = 0;
|
||||
//! The oversampling rate, ekf is 2k / this
|
||||
static uint8_t adc_oversampling = 18;
|
||||
static uint8_t adc_oversampling = 15;
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -282,9 +285,12 @@ int main()
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
timer_start();
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while (1) {
|
||||
|
||||
// Alive signal
|
||||
if((total_conversion_blocks % 100) == 0)
|
||||
PIOS_LED_Toggle(LED1);
|
||||
@ -300,11 +306,16 @@ int main()
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
#endif
|
||||
// Delay for valid data
|
||||
idle_counter = 0;
|
||||
do {
|
||||
idle_counter ++;
|
||||
//process_spi_request();
|
||||
} while ( ahrs_state != AHRS_DATA_READY );
|
||||
|
||||
counter_val = timer_count();
|
||||
running_counts = counter_val - last_counter_idle_end;
|
||||
last_counter_idle_start = counter_val;
|
||||
|
||||
while ( ahrs_state != AHRS_DATA_READY );
|
||||
|
||||
counter_val = timer_count();
|
||||
idle_counts = counter_val - last_counter_idle_start;
|
||||
last_counter_idle_end = counter_val;
|
||||
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
|
||||
@ -802,8 +813,7 @@ void process_spi_request(void)
|
||||
user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];
|
||||
|
||||
// compute the idle fraction
|
||||
user_tx_v1.payload.user.v.rsp.update.load = ((float) ekf_too_slow / (float) total_conversion_blocks) * 100;
|
||||
//(MAX_IDLE_COUNT - idle_counter) * 100 / MAX_IDLE_COUNT;
|
||||
user_tx_v1.payload.user.v.rsp.update.load = ((float) running_counts / (float) (idle_counts+running_counts)) * 100;
|
||||
|
||||
dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
|
||||
61
flight/AHRS/ahrs_timer.c
Normal file
61
flight/AHRS/ahrs_timer.c
Normal file
@ -0,0 +1,61 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @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
|
||||
*/
|
||||
|
||||
#include "ahrs_timer.h"
|
||||
|
||||
#define TIMER_RATE (8e6 / 128)
|
||||
void timer_start()
|
||||
{
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,ENABLE);
|
||||
PWR_BackupAccessCmd(ENABLE);
|
||||
|
||||
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
|
||||
RCC_RTCCLKCmd(ENABLE);
|
||||
RTC_WaitForLastTask();
|
||||
RTC_WaitForSynchro();
|
||||
RTC_WaitForLastTask();
|
||||
RTC_SetPrescaler(0); // counting at 8e6 / 128
|
||||
RTC_WaitForLastTask();
|
||||
RTC_SetCounter(0);
|
||||
RTC_WaitForLastTask();
|
||||
}
|
||||
|
||||
uint32_t timer_count()
|
||||
{
|
||||
return RTC_GetCounter();
|
||||
}
|
||||
|
||||
uint32_t timer_rate()
|
||||
{
|
||||
return TIMER_RATE;
|
||||
}
|
||||
43
flight/AHRS/inc/ahrs_timer.h
Normal file
43
flight/AHRS/inc/ahrs_timer.h
Normal file
@ -0,0 +1,43 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @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
|
||||
*/
|
||||
|
||||
#ifndef AHRS_TIMER
|
||||
#define AHRS_TIMER
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
void timer_start();
|
||||
uint32_t timer_count();
|
||||
uint32_t timer_rate();
|
||||
|
||||
#endif
|
||||
Loading…
x
Reference in New Issue
Block a user