Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'paparazzi/master' into telemetry

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-11-05 18:00:16 +01:00
parent be993f6b64 a0350247b3
commit c37d31a390
125 changed files with 2041 additions and 5641 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
+15
View File
@@ -89,6 +89,21 @@ void gpio_enable_clock(uint32_t port) {
case GPIOD:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPDEN);
break;
case GPIOE:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPEEN);
break;
case GPIOF:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPFEN);
break;
case GPIOG:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPGEN);
break;
case GPIOH:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPHEN);
break;
case GPIOI:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPIEN);
break;
default:
break;
};
sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
+136
View File
@@ -82,6 +82,54 @@ PRINT_CONFIG_MSG("Using TIM2 for PPM input.")
#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
#endif
#elif USE_PPM_TIM3
PRINT_CONFIG_MSG("Using TIM3 for PPM input.")
#define PPM_RCC &RCC_APB1ENR
#define PPM_PERIPHERAL RCC_APB1ENR_TIM3EN
#define PPM_TIMER TIM3
#ifdef STM32F4
/* Since APB prescaler != 1 :
* Timer clock frequency (before prescaling) is twice the frequency
* of the APB domain to which the timer is connected.
*/
#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
#endif
#elif USE_PPM_TIM4
PRINT_CONFIG_MSG("Using TIM4 for PPM input.")
#define PPM_RCC &RCC_APB1ENR
#define PPM_PERIPHERAL RCC_APB1ENR_TIM4EN
#define PPM_TIMER TIM4
#ifdef STM32F4
/* Since APB prescaler != 1 :
* Timer clock frequency (before prescaling) is twice the frequency
* of the APB domain to which the timer is connected.
*/
#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
#endif
#elif USE_PPM_TIM5
PRINT_CONFIG_MSG("Using TIM5 for PPM input.")
#define PPM_RCC &RCC_APB1ENR
#define PPM_PERIPHERAL RCC_APB1ENR_TIM5EN
#define PPM_TIMER TIM5
#ifdef STM32F4
/* Since APB prescaler != 1 :
* Timer clock frequency (before prescaling) is twice the frequency
* of the APB domain to which the timer is connected.
*/
#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
#endif
#elif USE_PPM_TIM1
PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
@@ -94,6 +142,19 @@ PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2)
#endif
#elif USE_PPM_TIM8
PRINT_CONFIG_MSG("Using TIM8 for PPM input.")
#define PPM_RCC &RCC_APB2ENR
#define PPM_PERIPHERAL RCC_APB2ENR_TIM8EN
#define PPM_TIMER TIM8
#ifdef STM32F4
#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2)
#endif
#else
#error Unknown PPM input timer configuration.
#endif
@@ -171,6 +232,59 @@ void tim2_isr(void) {
}
#elif USE_PPM_TIM3
void tim3_isr(void) {
if((TIM3_SR & PPM_CC_IF) != 0) {
timer_clear_flag(TIM3, PPM_CC_IF);
uint32_t now = timer_get_counter(TIM3) + timer_rollover_cnt;
DecodePpmFrame(now);
}
else if((TIM3_SR & TIM_SR_UIF) != 0) {
timer_rollover_cnt+=(1<<16);
timer_clear_flag(TIM3, TIM_SR_UIF);
}
}
#elif USE_PPM_TIM4
void tim4_isr(void) {
if((TIM4_SR & PPM_CC_IF) != 0) {
timer_clear_flag(TIM4, PPM_CC_IF);
uint32_t now = timer_get_counter(TIM4) + timer_rollover_cnt;
DecodePpmFrame(now);
}
else if((TIM4_SR & TIM_SR_UIF) != 0) {
timer_rollover_cnt+=(1<<16);
timer_clear_flag(TIM4, TIM_SR_UIF);
}
}
#elif USE_PPM_TIM5
void tim5_isr(void) {
if((TIM5_SR & PPM_CC_IF) != 0) {
timer_clear_flag(TIM5, PPM_CC_IF);
uint32_t now = timer_get_counter(TIM5) + timer_rollover_cnt;
DecodePpmFrame(now);
}
else if((TIM5_SR & TIM_SR_UIF) != 0) {
timer_rollover_cnt+=(1<<16);
timer_clear_flag(TIM5, TIM_SR_UIF);
}
}
#elif USE_PPM_TIM1
#if defined(STM32F1)
@@ -193,5 +307,27 @@ void tim1_cc_isr(void) {
}
}
#elif USE_PPM_TIM8 && defined(STM32F4)
#if defined(STM32F1)
void tim8_up_isr(void) {
#elif defined(STM32F4)
void tim8_up_tim13_isr(void) {
#endif
if((TIM8_SR & TIM_SR_UIF) != 0) {
timer_rollover_cnt+=(1<<16);
timer_clear_flag(TIM8, TIM_SR_UIF);
}
}
void tim8_cc_isr(void) {
if((TIM8_SR & PPM_CC_IF) != 0) {
timer_clear_flag(TIM8, PPM_CC_IF);
uint32_t now = timer_get_counter(TIM8) + timer_rollover_cnt;
DecodePpmFrame(now);
}
}
#endif /* USE_PPM_TIM1 */
sw/airborne/boards/stm32f4_discovery.h
+16 -5
View File
@@ -7,9 +7,9 @@
* PA2 = PWM7
* PA3 = PWM6
* PA4 = ADC_4 (ADC12 IN 4)+CS43L22 DAC out, capacitively coupled+100Kohm, should not interfere.
* PA5 = FREE
* PA6 = CANNOT BE USED
* PA7 = FREE
* PA5 = SPI1 SCK if STM32F4_DISCOVERY_SPI1_FOR_LIS302
* PA6 = SPI1 MISO if STM32F4_DISCOVERY_SPI1_FOR_LIS302
* PA7 = SPI1 MOSI if STM32F4_DISCOVERY_SPI1_FOR_LIS302
* PA8 = SPECTRUM BIND
* PA9 = FREE (ONLY if usb is not active during runtime, PC0 must be high or input )
* PA10 = UART2 (Spektrum input)
@@ -73,7 +73,7 @@
* PE0 = CANNOT BE USED (Accel int output)
* PE1 = CANNOT BE USED (Accel int output)
* PE2 = SPI SLAVE 0
* PE3 = FREE (Needs some testing as it is used for the accel spi/i2c select pin)
* PE3 = SPI SLAVE 2 (used for the LIS302DL accel spi/i2c select pin)
* PE4 = FREE
* PE5 = PWM4
* PE6 = PWM5
@@ -214,6 +214,15 @@
/************************************** SPI *************************************************/
/***************************************************************************************************/
#if STM32F4_DISCOVERY_SPI1_FOR_LIS302
#define SPI1_GPIO_AF GPIO_AF5
#define SPI1_GPIO_PORT_SCK GPIOA
#define SPI1_GPIO_SCK GPIO5
#define SPI1_GPIO_PORT_MISO GPIOA
#define SPI1_GPIO_MISO GPIO6
#define SPI1_GPIO_PORT_MOSI GPIOA
#define SPI1_GPIO_MOSI GPIO7
#else
#define SPI1_GPIO_AF GPIO_AF5
#define SPI1_GPIO_PORT_SCK GPIOB
#define SPI1_GPIO_SCK GPIO3
@@ -221,7 +230,7 @@
#define SPI1_GPIO_MISO GPIO4
#define SPI1_GPIO_PORT_MOSI GPIOB
#define SPI1_GPIO_MOSI GPIO5
#endif
/* CANNOT BE USED IF PWM CHANNELS 10 & 11 ARE ACTIVE !!! */
#define SPI2_GPIO_AF GPIO_AF5
@@ -245,6 +254,8 @@
#define SPI_SELECT_SLAVE0_PIN GPIO2
#define SPI_SELECT_SLAVE1_PORT GPIOE
#define SPI_SELECT_SLAVE1_PIN GPIO7
#define SPI_SELECT_SLAVE2_PORT GPIOE
#define SPI_SELECT_SLAVE2_PIN GPIO3
/***************************************************************************************************/
sw/airborne/booz/test/Makefile
-57
View File
@@ -1,57 +0,0 @@
##
# $Id$
#
# Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
#
# This file is part of paparazzi.
#
# paparazzi 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 2, or (at your option)
# any later version.
#
# paparazzi 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 paparazzi; see the file COPYING. If not, write to
# the Free Software Foundation, 59 Temple Place - Suite 330,
# Boston, MA 02111-1307, USA.
##
# Quiet compilation
Q=@
CC = gcc
CFLAGS = -std=gnu99 -Wall -I.. -I../.. -I../../test/ -I../../../include -I../../booz_priv
LDFLAGS = -lm
CFLAGS += -I../../../../var/BOOZ2_A1P
test_mlkf: test_mlkf.c ../booz_ahrs.c ../../booz_priv/ahrs/booz_ahrs_mlkf.c ../../booz_priv/ahrs/booz_ahrs_mlkf_opt.c ../booz_imu.c ../../math/pprz_trig_int.c ./imu_dummy.c ../ahrs/booz_ahrs_aligner.c
$(CC) $(CFLAGS) -DBOOZ_IMU_TYPE_H=\"test/imu_dummy.h\" -o $@ $^ $(LDFLAGS)
test_vg_ref: test_vg_ref.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_vg_adpt: test_vg_adpt.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_deadband: test_deadband.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_scaling: test_scaling.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
TEST_ATT_CFLAGS = -DSTABILISATION_ATTITUDE_TYPE_INT \
-DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\" \
-DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_int.h\"
test_att_ref: test_att_ref.c ../stabilization/booz_stabilization_attitude_ref_quat_int.c
$(CC) $(CFLAGS) $(TEST_ATT_CFLAGS) -I/home/poine/work/savannah/paparazzi3/trunk/var/BOOZ2_A1 -o $@ $^ $(LDFLAGS)
clean:
$(Q)rm -f *~ test_att_ref
sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
-76
View File
@@ -1,76 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <inttypes.h>
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "interrupt_hw.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/datalink/datalink.h"
#include "booz2_test_buss_bldc_hexa.h"
#define NB_MOTORS 6
static const uint8_t motor_addr[] = {0x52, 0x54, 0x56, 0x58, 0x5A, 0x5C};
uint8_t motor = 0;
uint8_t thrust = 10;
static bool_t i2c_done;
//static uint8_t addr = 0x52; /* 1 : back right */
//static uint8_t addr = 0x54; /* 2 : back left bad balanced */
//static uint8_t addr = 0x56; /* 3 : center right not so well balanced */
//static uint8_t addr = 0x58; /* 4 : center left */
//static uint8_t addr = 0x5A; /* 5 : front right */
//static uint8_t addr = 0x5C; /* 6 : front left */
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
int main( void ) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
i2c0_buf[0] = thrust;
i2c0_transmit(motor_addr[motor], 1, &i2c_done);
RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
}
static inline void main_event_task( void ) {
DatalinkEvent();
}
sw/airborne/booz/test/booz2_test_buss_bldc_hexa.h
-7
View File
@@ -1,7 +0,0 @@
#ifndef BOOZ2_TEST_BUSS_BLDC_HEXA_H
#define BOOZ2_TEST_BUSS_BLDC_HEXA_H
extern uint8_t motor;
extern uint8_t thrust;
#endif /* BOOZ2_TEST_BUSS_BLDC_HEXA_H */
sw/airborne/booz/test/booz2_test_crista.c
-117
View File
@@ -1,117 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <inttypes.h>
#include "std.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/imu.h"
#include "interrupt_hw.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
static inline void on_imu_event(void);
int main( void ) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
/* LED_ON(4); */
/* LED_ON(5); */
/* LED_ON(6); */
/* LED_ON(7); */
uart0_init();
imu_impl_init();
imu_init();
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
//#if 0
RunOnceEvery(100, {
// LED_TOGGLE(7);
DOWNLINK_SEND_ALIVE(16, MD5SUM);
});
// uint16_t foo = ami601_status;
// DOWNLINK_SEND_BOOT(&foo);
//#endif
// if (sys_time.nb_sec > 2)
imu_periodic();
}
static inline void main_event_task( void ) {
ImuEvent(on_imu_event);
}
static inline void on_imu_event(void) {
ImuScaleGyro();
ImuScaleAccel();
// LED_TOGGLE(6);
static uint8_t cnt;
cnt++;
if (cnt > 15) cnt = 0;
if (cnt == 0) {
DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_unscaled.x,
&imu_gyro_unscaled.y,
&imu_gyro_unscaled.z);
DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_unscaled.x,
&imu_accel_unscaled.y,
&imu_accel_unscaled.z);
}
else if (cnt == 7) {
DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x,
&imu_gyro.y,
&imu_gyro.z);
DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x,
&imu_accel.y,
&imu_accel.z);
}
}
sw/airborne/booz/test/booz2_test_gps.c
-85
View File
@@ -1,85 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <inttypes.h>
#include "std.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/gps.h"
#include "interrupt_hw.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
static void on_gps_sol(void);
int main( void ) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
gps_init();
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
RunOnceEvery(128, { LED_PERIODIC();});
}
static inline void main_event_task( void ) {
GpsEvent(on_gps_sol);
}
static void on_gps_sol(void) {
DOWNLINK_SEND_GPS_INT( DefaultChannel, DefaultDevice,
&gps.ecef_pos.x,
&gps.ecef_pos.y,
&gps.ecef_pos.z,
&gps.lla_pos.lat,
&gps.lla_pos.lon,
&gps.lla_pos.alt,
&gps.ecef_vel.x,
&gps.ecef_vel.y,
&gps.ecef_vel.z,
&gps.pacc,
&gps.sacc,
&gps.tow,
&gps.pdop,
&gps.num_sv,
&gps.fix);
}
sw/airborne/booz/test/booz2_test_max1168.c
-141
View File
@@ -1,141 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <inttypes.h>
#include "std.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "interrupt_hw.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "armVIC.h"
#include "LPC21xx.h"
#include "peripherals/max1168.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
static void main_init_ssp(void);
static void SSP_ISR(void) __attribute__((naked));
int main( void ) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
main_init_ssp();
max1168_init();
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
LED_TOGGLE(3);
max1168_read();
}
static inline void main_event_task( void ) {
if (max1168_status == MAX1168_DATA_AVAILABLE) {
RunOnceEvery(10, {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); });
max1168_status = MAX1168_IDLE;
}
}
/* SSPCR0 settings */
#define SSP_DDS 0x0F << 0 /* data size : 16 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
#define SSP_MS 0x00 << 2 /* master slave mode : master */
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
#define SSP_PINSEL1_SCK (2<<2)
#define SSP_PINSEL1_MISO (2<<4)
#define SSP_PINSEL1_MOSI (2<<6)
#define SSP_Enable() SetBit(SSPCR1, SSE);
#define SSP_Disable() ClearBit(SSPCR1, SSE);
#define SSP_EnableRxi() SetBit(SSPIMSC, RXIM)
#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM)
#define SSP_EnableTxi() SetBit(SSPIMSC, TXIM)
#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM)
#define SSP_EnableRti() SetBit(SSPIMSC, RTIM);
#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM);
#define SSP_ClearRti() SetBit(SSPICR, RTIC);
#ifndef SSP_VIC_SLOT
#define SSP_VIC_SLOT 7
#endif
static void main_init_ssp(void) {
/* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
}
static void SSP_ISR(void) {
ISR_ENTRY();
Max1168OnSpiInt();
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
sw/airborne/booz/test/plot_scaling.sce
-69
View File
@@ -1,69 +0,0 @@
///
// $Id$
//
// Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
//
// This file is part of paparazzi.
//
// paparazzi 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 2, or (at your option)
// any later version.
//
// paparazzi 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 paparazzi; see the file COPYING. If not, write to
// the Free Software Foundation, 59 Temple Place - Suite 330,
// Boston, MA 02111-1307, USA.
///
clear();
M=fscanfMat('traj.out');
value = M(:,1);
raw_sensor_f = M(:,2);
raw_sensor_i = M(:,3);
scaled_sensor_f = M(:,4);
scaled_sensor_i = M(:,5);
time=1:length(value);
//k = find(time > 500 & time < 550);
k = find(value > -0.009 & value < 0.009);
//k = 1:length(time);
scf();
//clf();
drawlater();
subplot(3,1,1);
xtitle('real', 'time (s)','');
plot2d(time(k), value(k), 3);
plot2d2(time(k), scaled_sensor_i(k)/2^10, 2);
legends(["float", "int", "fixed"],[5 3 2], with_box=%f, opt="lr");
subplot(3,1,2);
xtitle('raw', 'time (s)','');
plot2d(time(k), raw_sensor_f(k), 3);
plot2d2(time(k), raw_sensor_i(k), 2);
legends(["float", "int", "fixed"],[5 3 2], with_box=%f, opt="lr");
subplot(3,1,3);
xtitle('scaled', 'time (s)','');
plot2d(value(k), scaled_sensor_f(k), 3);
plot2d2(value(k), scaled_sensor_i(k), 5);
//plot2d(time(k), zeros(1, length(time(k))), 1);
xgrid(1);
drawnow();
sw/airborne/booz/test/plot_test_vg_adpt.sce
-93
View File
@@ -1,93 +0,0 @@
///
// $Id$
//
// Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
//
// This file is part of paparazzi.
//
// paparazzi 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 2, or (at your option)
// any later version.
//
// paparazzi 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 paparazzi; see the file COPYING. If not, write to
// the Free Software Foundation, 59 Temple Place - Suite 330,
// Boston, MA 02111-1307, USA.
///
clear();
M=fscanfMat('traj.out');
time = M(:,1);
meas = M(:,2);
thrust = M(:,3);
zdd_ref = M(:,4);
ffX = M(:,5);
ffP = M(:,6);
ffm = M(:,7);
ifX = M(:,8);
ifP = M(:,9);
//k = find(time > 500 & time < 550);
//k = find(time > 1110 & time < 1168);
k = 1:length(time);
clf();
drawlater();
subplot(3,2,1);
xtitle('X', 'time (s)','');
plot2d(time(k), ffm(k), 4);
plot2d(time(k), ffX(k), 3);
plot2d(time(k), ifX(k)/2^18, 5);
legends(["float", "int", "meas"],[3 5 4], with_box=%f, opt="lr");
subplot(3,2,2);
xtitle('P', 'time (s)','');
plot2d(time(k), ffP(k), 3);
plot2d(time(k), ifP(k)/2^18, 5);
legends(["float", "int"],[3 5], with_box=%f, opt="lr");
subplot(3,2,3);
xtitle('X', 'time (s)','');
plot2d(time(k), ffX(k)*2^18, 3);
plot2d(time(k), ifX(k), 5);
legends(["float", "int"],[3 5], with_box=%f, opt="lr");
subplot(3,2,4);
xtitle('P', 'time (s)','');
plot2d(time(k), ffP(k)*2^18, 3);
plot2d(time(k), ifP(k), 5);
legends(["float", "int"],[3 5], with_box=%f, opt="lr");
subplot(3,2,5);
xtitle('Zdd', 'time (s)','');
plot2d(time(k), meas(k)/2^10, 1);
subplot(3,2,6);
xtitle('Thrust', 'time (s)','');
fmg = 9.81./ffX(k);
img = 9.81./ifX(k)*2^18;
plot2d(time(k), fmg, 3);
plot2d(time(k), img, 5);
fmzdd = zdd_ref(k)./ffX(k)./2^10;
imzdd = zdd_ref(k)./ifX(k)./2^10*2^18;
//plot2d(time(k), fmg - fmzdd, 3);
//plot2d(time(k), img - imzdd, 5);
//plot2d(time(k), thrust(k), 2);
legends(["float", "int"],[3 5], with_box=%f, opt="lr");
drawnow();
sw/airborne/booz/test/plot_test_vg_ref.sce
-63
View File
@@ -1,63 +0,0 @@
///
// $Id$
//
// Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
//
// This file is part of paparazzi.
//
// paparazzi 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 2, or (at your option)
// any later version.
//
// paparazzi 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 paparazzi; see the file COPYING. If not, write to
// the Free Software Foundation, 59 Temple Place - Suite 330,
// Boston, MA 02111-1307, USA.
///
clear();
M=fscanfMat('traj.out');
time = M(:,1);
fzsp = M(:,2);
fzdsp = M(:,3);
fz = M(:,4);
fzd = M(:,5);
fzdd = M(:,6);
iz = M(:,7);
izd = M(:,8);
izdd = M(:,9);
clf();
drawlater();
subplot(3,1,1);
xtitle('z', 'time (s)','');
plot2d(time, fzsp, 1);
plot2d(time, fz, 2);
plot2d(time, iz, 3);
legends(["sp", "float", "int"],[1 2 3], with_box=%f, opt="ur");
subplot(3,1,2);
xtitle('zd', 'time (s)','');
plot2d(time, fzdsp, 1);
plot2d(time, fzd, 2);
plot2d(time, izd, 3);
legends(["float", "int"],[2 3], with_box=%f, opt="lr");
subplot(3,1,3);
xtitle('zdd', 'time (s)','');
plot2d(time, fzdd, 2);
plot2d(time, izdd, 3);
legends(["float", "int"],[2 3], with_box=%f, opt="lr");
drawnow();
sw/airborne/booz/test/test_mlkf.c
-116
View File
@@ -1,116 +0,0 @@
#include <stdio.h>
#include <string.h>
#include "math/pprz_algebra_double.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "ahrs/ahrs_mlkf.h"
static void read_data(const char* filename);
static void feed_imu(int i);
static void store_filter_output(int i);
static void dump_output(const char* filename);
#define IN_FILE "../../../simulator/scilab/q6d/data/stop_stop_state_sensors.txt"
#define OUT_FILE "./out.txt"
#define MAX_SAMPLE 15000
struct test_sample {
double time;
struct DoubleQuat quat_true;
struct DoubleRates omega_true;
struct DoubleRates gyro;
struct DoubleVect3 accel;
struct DoubleVect3 mag;
};
static struct test_sample samples[MAX_SAMPLE];
static int nb_samples;
struct test_output {
struct FloatQuat quat_est;
struct FloatRates bias_est;
struct FloatRates rate_est;
float P[6][6];
};
static struct test_output output[MAX_SAMPLE];
int main(int argc, char** argv) {
read_data(IN_FILE);
imu_init();
ahrs_init();
for (int i=0; i<nb_samples; i++) {
feed_imu(i);
ahrs_propagate();
ahrs_update_accel();
ahrs_update_mag();
store_filter_output(i);
}
dump_output(OUT_FILE);
return 0;
}
static void read_data(const char* filename) {
FILE* fd = fopen(filename, "r");
nb_samples = 0;
int ret = 0;
do {
struct test_sample* s = &samples[nb_samples];
ret = fscanf(fd, "%lf [%lf %lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf]",
&s->time,
&s->quat_true.qi, &s->quat_true.qx, &s->quat_true.qy, &s->quat_true.qz,
&s->omega_true.p, &s->omega_true.q, &s->omega_true.r,
&s->gyro.p, &s->gyro.q, &s->gyro.r,
&s->accel.x, &s->accel.y, &s->accel.z,
&s->mag.x, &s->mag.y, &s->mag.z );
nb_samples++;
}
while (ret == 17 && nb_samples < MAX_SAMPLE);
nb_samples--;
fclose(fd);
printf("read %d points in file %s\n", nb_samples, filename);
}
static void feed_imu(int i) {
if (i>0) {
RATES_COPY(imu.gyro_prev, imu.gyro);
}
else {
RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro);
}
RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
}
static void store_filter_output(int i) {
QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
RATES_COPY(output[i].bias_est, ahrs_mlkf.gyro_bias);
RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
memcpy(output[i].P, ahrs_mlkf.P, sizeof(ahrs_mlkf.P));
}
static void dump_output(const char* filename) {
FILE* fd = fopen(filename, "w");
int i;
for (i=0; i<nb_samples; i++) {
fprintf(fd, "%.16f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f %.16f %.16f %.16f]\n",
samples[i].time,
output[i].quat_est.qi, output[i].quat_est.qx, output[i].quat_est.qy, output[i].quat_est.qz, // quaternion
output[i].rate_est.p, output[i].rate_est.q, output[i].rate_est.r, // omega
output[i].bias_est.p, output[i].bias_est.q, output[i].bias_est.r, // bias
output[i].P[0][0], output[i].P[1][1], output[i].P[2][2], // covariance
output[i].P[3][3], output[i].P[4][4], output[i].P[5][5] );
}
fclose(fd);
printf("wrote %d points in file %s\n", nb_samples, filename);
}
sw/airborne/booz/test/test_mlkf.sce
-121
View File
@@ -1,121 +0,0 @@
clear();
clearglobal();
exec('../../../simulator/scilab/q6d/q6d_fdm.sci');
exec('../../../simulator/scilab/q6d/q6d_algebra.sci');
global ahrs_quat;
global ahrs_omega;
global ahrs_bias;
global ahrs_cov;
function read_ahrs_output(filename)
global ahrs_quat;
global ahrs_omega;
global ahrs_bias;
global ahrs_cov;
ahrs_quat = zeros(4,length(time));
ahrs_omega = zeros(3,length(time));
ahrs_bias = zeros(3,length(time));
ahrs_cov = zeros(6,length(time));
i=1;
u=mopen(filename, 'r');
while meof(u) == 0 & i <= length(time)
line = mgetl(u, 1);
if line ~= []
[nb_scan, t, qi, qx, qy, qz, p, q, r, bp, bq, br, c11, c22, c33, c44, c55, c66] = ...
msscanf(1, line, '%f [%f %f %f %f] [%f %f %f] [%f %f %f] [%f %f %f %f %f %f]');
if nb_scan == 17
ahrs_quat(:,i) = [qi qx qy qz]';
ahrs_omega(:,i) = [p q r]';
ahrs_bias(:,i) = [bp bq br]';
ahrs_cov(:,i) = [c11 c22 c33 c44 c55 c66]';
i = i+1;
end
end
end
mclose(u);
endfunction
function display_ahrs_state()
clf();
f=get("current_figure");
f.figure_name="AHRS state";
eul_fdm = zeros(3,length(time));
eul_ahrs = zeros(3,length(time));
for i=1:length(time)
eul_fdm(:,i) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,i));
eul_ahrs(:,i) = euler_of_quat(ahrs_quat(:,i));
end
subplot(4,3,1);
plot2d(time, deg_of_rad(eul_ahrs(1,:)), 3);
plot2d(time, deg_of_rad(eul_fdm(1,:)), 2);
legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul");
xtitle('Phi');
subplot(4,3,2);
plot2d(time, deg_of_rad(eul_ahrs(2,:)), 3);
plot2d(time, deg_of_rad(eul_fdm(2,:)), 2);
legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul");
xtitle('Theta');
subplot(4,3,3);
plot2d(time, deg_of_rad(eul_ahrs(3,:)), 3);
plot2d(time, deg_of_rad(eul_fdm(3,:)), 2);
legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul");
xtitle('Psi');
subplot(4,3,4);
plot2d(time, deg_of_rad(ahrs_omega(1,:)), 3);
plot2d(time, deg_of_rad(fdm_state(FDM_SP,:)), 2);
xtitle('p');
subplot(4,3,5);
plot2d(time, deg_of_rad(ahrs_omega(2,:)), 3);
plot2d(time, deg_of_rad(fdm_state(FDM_SQ,:)), 2);
xtitle('q');
subplot(4,3,6);
plot2d(time, deg_of_rad(ahrs_omega(3,:)), 3);
plot2d(time, deg_of_rad(fdm_state(FDM_SR,:)), 2);
xtitle('r');
subplot(4,3,7);
plot2d(time, deg_of_rad(ahrs_bias(1,:)), 3);
xtitle('bp');
subplot(4,3,8);
plot2d(time, deg_of_rad(ahrs_bias(2,:)), 3);
xtitle('bq');
subplot(4,3,9);
plot2d(time, deg_of_rad(ahrs_bias(3,:)), 3);
xtitle('br');
subplot(4,3,10);
subplot(4,3,11);
plot2d(time, ahrs_cov(1,:),3);
plot2d(time, ahrs_cov(2,:),3);
plot2d(time, ahrs_cov(3,:),3);
legends(["ahrs_c", "ahrs_s"],[3 4], with_box=%f, opt="ul");
xtitle('cov angles');
subplot(4,3,12);
plot2d(time, ahrs_cov(4,:),3);
plot2d(time, ahrs_cov(5,:),3);
plot2d(time, ahrs_cov(6,:),3);
xtitle('cov bias');
endfunction
load('../../../simulator/scilab/q6d/data/stop_stop_state_sensors.dat', 'time', 'fdm_state', 'sensor_gyro', 'sensor_accel', 'sensor_mag');
read_ahrs_output('out.txt');
display_ahrs_state();
sw/airborne/booz/test/test_scaling.c
-117
View File
@@ -1,117 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdio.h>
#include <math.h>
#include <inttypes.h>
#include "math/pprz_algebra_int.h"
#define IMU_ACCEL_X_NEUTRAL 32081
#define IMU_ACCEL_X_SENS -2.50411474
#define IMU_ACCEL_X_SENS_NUM -10650
#define IMU_ACCEL_X_SENS_DEN 4253
void test_1(void);
void test_2(void);
void test_3(void);
int main(int argc, char** argv){
// test_2();
test_3();
return 0;
}
void test_1(void) {
double value_f;
for (value_f=-9.81; value_f<9.81; value_f += 0.001) {
double neutral_f = (double)IMU_ACCEL_X_NEUTRAL;
double sensitivity_f = 1./IMU_ACCEL_X_SENS;
double sensor_raw_f = ACCEL_BFP_OF_REAL(value_f) * sensitivity_f + neutral_f;
int32_t sensor_raw_i = rint(sensor_raw_f);
double scaled_sensor_f = ACCEL_BFP_OF_REAL(value_f);
#if 1
int32_t scaled_sensor_i = ((sensor_raw_i - IMU_ACCEL_X_NEUTRAL) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
#endif
#if 0
int32_t scaled_sensor_i = (sensor_raw_i * IMU_ACCEL_X_SENS_NUM / IMU_ACCEL_X_SENS_DEN) -
(IMU_ACCEL_X_NEUTRAL * IMU_ACCEL_X_SENS_NUM / IMU_ACCEL_X_SENS_DEN);
#endif
#if 0
const int32_t delta = (sensor_raw_i - IMU_ACCEL_X_NEUTRAL);
int32_t scaled_sensor_i;
if (delta > 0)
scaled_sensor_i = ( delta * IMU_ACCEL_X_SENS_NUM ) / IMU_ACCEL_X_SENS_DEN;
else
scaled_sensor_i = ( delta * IMU_ACCEL_X_SENS_NUM + (IMU_ACCEL_X_SENS_DEN>>1)) / IMU_ACCEL_X_SENS_DEN;
#endif
printf("%f %f %d %f %d\n", value_f, sensor_raw_f, sensor_raw_i, scaled_sensor_f, scaled_sensor_i);
}
}
void test_2(void) {
int a;
for (a=-7; a<7; a++) {
int b = a/-2;
int c = (a>0 ? a+1 : a-1)/-2;
double d = rint((double)a/-2.);
printf("%- d %- d %- d %- .1f\n", a, b, c, d);
}
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
#define N_OFFSET 2
void test_3(void) {
int a;
for (a=-(1<<N_OFFSET); a<=(1<<N_OFFSET); a++) {
int32_t b = (a>>N_OFFSET);
int32_t c;
// if ( a>=0 )
// c = (a+(1<<(N_OFFSET-1)))>>N_OFFSET;
// else
// c = (a+(1<<(N_OFFSET-1))-1)>>N_OFFSET;
c = OFFSET_AND_ROUND(a, N_OFFSET);
int32_t d;
d = OFFSET_AND_ROUND2(a, N_OFFSET);
double e;
e = (double)a/(double)(1<<N_OFFSET);
printf("%- d %- d %- d %- d %.1f\n", a, b, c, d, e);
}
}
sw/airborne/booz/test/test_vg_adpt.c
-173
View File
@@ -1,173 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include "std.h"
#include "booz_geometry_mixed.h"
#define GUIDANCE_V_C
#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
#define NB_STEP 20000
int n_dat;
double time[NB_STEP];
int32_t z_sp[NB_STEP];
int32_t zd_sp[NB_STEP];
int32_t est_z[NB_STEP];
int32_t est_zd[NB_STEP];
int32_t est_zdd[NB_STEP];
int32_t ref_z[NB_STEP];
int32_t ref_zd[NB_STEP];
int32_t ref_zdd[NB_STEP];
int32_t adp_inv_m[NB_STEP];
int32_t adp_cov[NB_STEP];
int32_t sum_err[NB_STEP];
int32_t ff_cmd[NB_STEP];
int32_t fb_cmd[NB_STEP];
int32_t delta_t[NB_STEP];
int32_t ifX[NB_STEP];
int32_t ifP[NB_STEP];
double ffX[NB_STEP];
double ffm[NB_STEP];
double ffP[NB_STEP];
static void float_filter_init(void) {
ffX[0] = GV_ADAPT_X0_F;
ffP[0] = GV_ADAPT_P0_F;
}
static void float_filter_run( int i) {
int prev = i>0 ? i-1 : i;
ffX[i] = ffX[prev];
ffP[i] = ffP[prev];
if (delta_t[prev] == 0) return;
ffP[i] = ffP[i] + GV_ADAPT_SYS_NOISE_F;
ffm[i] = (9.81 - (double)est_zdd[i]/(double)(1<<10)) / (double)delta_t[prev];
double residual = ffm[i] - ffX[i];
double E = ffP[i] + GV_ADAPT_MEAS_NOISE_F;
double K = ffP[i] / E;
ffP[i] = ffP[i] - K * ffP[i];
ffX[i] = ffX[i] + K * residual;
}
static int read_data(const char* filename) {
FILE *fp = NULL;
fp = fopen(filename, "r");
if (fp == NULL)
return -1;
char * line = NULL;
size_t len = 0;
size_t read;
n_dat = 0;
while ((read = getline(&line, &len, fp)) != -1 && n_dat< NB_STEP) {
if (sscanf(line, "%lf %*d VERT_LOOP %d %d %d %d %d %d %d %d %d %d %d %d %d %d",
&time[n_dat],
&z_sp[n_dat], &zd_sp[n_dat],
&est_z[n_dat], &est_zd[n_dat], &est_zdd[n_dat],
&ref_z[n_dat], &ref_zd[n_dat], &ref_zdd[n_dat],
&adp_inv_m[n_dat], &adp_cov[n_dat], &sum_err[n_dat],
&ff_cmd[n_dat], &fb_cmd[n_dat], &delta_t[n_dat]) == 15) n_dat++;
}
if (line)
free(line);
fclose(fp);
return 0;
}
void gen_data(void) {
int i = 0;
n_dat = NB_STEP;
while (i<n_dat) {
long int r = random();
double noise = 2. * 2. * ((double)r / (double)RAND_MAX - 0.5);
// printf("%ld %f\n", r, noise);
est_zdd[i] = 0 + noise;
delta_t[i] = 85;
i++;
}
}
void dump_res(void) {
int i = 0;
while (i<n_dat) {
printf("%f %d %d %d %f %f %f %d %d\n", time[i], est_zdd[i], delta_t[i], ref_zdd[i], ffX[i], ffP[i], ffm[i], ifX[i], ifP[i]);
i++;
}
}
#define FF_CMD_FRAC 18
void test_command(int i) {
const int32_t inv_m_i = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
int32_t cmd_i = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES))) / inv_m_i;
double inv_m_f = (double)BOOZ_FLOAT_OF_INT(gv_adapt_X, GV_ADAPT_X_FRAC);
double cmd_f = (9.81 - (double)BOOZ_FLOAT_OF_INT(ref_zdd[i], IACCEL_RES)) / inv_m_f;
int32_t cmd_i_fixed;
// = ((int32_t)BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES) - ref_zdd[i] + (inv_m_i_fixed>>1)) / inv_m_i_fixed;
if (ref_zdd[i] < BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES))
cmd_i_fixed = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES)) + (inv_m_i>>1) ) / inv_m_i;
else
cmd_i_fixed = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES)) - (inv_m_i>>1) ) / inv_m_i;
printf("%d %f %d\n",cmd_i, cmd_f, cmd_i_fixed);
}
int main(int argc, char** argv) {
// gen_data();
read_data("09_02_15__20_45_58.data");
printf("read %d\n", n_dat);
gv_adapt_init();
float_filter_init();
int i = 0;
while (i<n_dat) {
Bound(delta_t[i], 1, 200);
gv_adapt_run(est_zdd[i], delta_t[i]);
ifX[i] = gv_adapt_X;
ifP[i] = gv_adapt_P;
float_filter_run(i);
// test_command(i);
i++;
}
dump_res();
return 0;
}
sw/airborne/booz/test/test_vg_ref.c
-116
View File
@@ -1,116 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdio.h>
#include "booz_geometry_mixed.h"
#define GUIDANCE_V_C
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#define NB_STEP 10000
#define DT (1./GV_FREQ)
double z_sp, zd_sp, z_ref, zd_ref, zdd_ref;
void set_ref(double z, double zd, double zdd) {
z_ref = z;
zd_ref = zd;
zdd_ref = zdd;
}
void update_ref_from_z(void) {
z_ref += (zd_ref * DT);
zd_ref += (zdd_ref * DT);
zdd_ref = -2.*GV_ZETA*GV_OMEGA*zd_ref - GV_OMEGA*GV_OMEGA*(z_ref - z_sp);
Bound(zdd_ref, GV_MIN_ZDD_F, GV_MAX_ZDD_F);
if (zd_ref <= GV_MIN_ZD_F) {
zd_ref = GV_MIN_ZD_F;
if (zdd_ref < 0)
zdd_ref = 0;
}
else if (zd_ref >= GV_MAX_ZD_F) {
zd_ref = GV_MAX_ZD_F;
if (zdd_ref > 0)
zdd_ref = 0;
}
}
void update_ref_from_zd(void) {
z_ref += (zd_ref * DT);
zd_ref += (zdd_ref * DT);
zdd_ref = -1/GV_REF_THAU_F*(zd_ref - zd_sp);
Bound(zdd_ref, GV_MIN_ZDD_F, GV_MAX_ZDD_F);
if (zd_ref <= GV_MIN_ZD_F) {
zd_ref = GV_MIN_ZD_F;
if (zdd_ref < 0)
zdd_ref = 0;
}
else if (zd_ref >= GV_MAX_ZD_F) {
zd_ref = GV_MAX_ZD_F;
if (zdd_ref > 0)
zdd_ref = 0;
}
}
void print_ref(int i) {
double i2f_z = BOOZ_FLOAT_OF_INT( gv_z_ref, GV_Z_REF_FRAC);
double i2f_zd = BOOZ_FLOAT_OF_INT( gv_zd_ref, GV_ZD_REF_FRAC);
double i2f_zdd = BOOZ_FLOAT_OF_INT( gv_zdd_ref, GV_ZDD_REF_FRAC);
printf("%f %f %f %f %f %f %f %f %f\n",
(double)i*DT, z_sp, zd_sp,
z_ref, zd_ref, zdd_ref,
i2f_z, i2f_zd, i2f_zdd );
}
int32_t get_sp (int i) {
// return BOOZ_INT_OF_FLOAT(i>512 ? -50.0 : 0, IPOS_FRAC);
return BOOZ_INT_OF_FLOAT((i>512&&i<3072) ? -1.0 : 0, ISPEED_RES);
}
int main(int argc, char** argv) {
gv_set_ref(0, 0, 0);
set_ref(0., 0., 0.);
int i = 0;
while (i<NB_STEP) {
int32_t sp_i = get_sp(i);
// gv_update_ref_from_z_sp(sp_i);
gv_update_ref_from_zd_sp(sp_i);
// z_sp = BOOZ_FLOAT_OF_INT(sp_i, IPOS_FRAC);
// update_ref_from_z();
zd_sp = BOOZ_FLOAT_OF_INT(sp_i, ISPEED_RES);
update_ref_from_zd();
print_ref(i);
i++;
}
return 0;
}
sw/airborne/booz/test/vert_loop.dat.bz2
BIN
View File
Binary file not shown.
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h
-3
View File
@@ -45,9 +45,6 @@ extern struct Int32Eulers stabilization_att_sum_err;
extern int32_t stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t stabilization_att_ff_cmd[COMMANDS_NB];
// common so it can be used for downlink/debug
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
#define stabilization_attitude_common_int_SetKiPhi(_val) { \
stabilization_gains.i.x = _val; \
stabilization_att_sum_err.phi = 0; \
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
-2
View File
@@ -27,6 +27,4 @@
#include "math/pprz_algebra_int.h"
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+1 -1
View File
@@ -29,8 +29,8 @@
#include "math/pprz_algebra_int.h"
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC
extern struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+3
View File
@@ -32,6 +32,9 @@
#include "stabilization_attitude_ref_int.h"
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC
void stabilization_attitude_ref_enter(void);
#endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */
sw/airborne/math/Makefile
+8 -6
View File
@@ -8,14 +8,15 @@ CC= gcc
CFLAGS= -fpic
INCLUDES= -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne
# build in ../../../var/build/math
ifndef BUILDDIR
BUILDDIR=../../../var/build/math
ifeq ($(PAPARAZZI_HOME),)
PAPARAZZI_HOME=$(PAPARAZZI_SRC)
endif
ifndef PREFIX
PREFIX=/usr
endif
# build in $(PAPARAZZI_HOME)/var/build/math
BUILDDIR ?= $(PAPARAZZI_HOME)/var/build/math
PREFIX ?= /usr/local
LIB_INSTALLDIR=${PREFIX}/lib
INCLUDE_INSTALLDIR=${PREFIX}/include/pprz
PKGCONFIG_INSTALLDIR=${PREFIX}/lib/pkgconfig
@@ -48,3 +49,4 @@ $(BUILDDIR)/%.o: %.c
clean:
$(Q)rm -f $(BUILDDIR)/*.o $(BUILDDIR)/$(LIBNAME).so
.PHONY: all clean shared_lib install_shared_lib
sw/airborne/math/README
+4 -4
View File
@@ -11,11 +11,11 @@ HOWTO install a shared library to use in other projects
2. Install library: in this folder, type
make install_shared_lib
the default install dir is /usr
the default install dir is /usr/local
and will install files in
/usr/lib
/usr/lib/pkgconfig
/usr/include/pprz
/usr/local/lib
/usr/local/lib/pkgconfig
/usr/local/include/pprz
to change the install dir: PREFIX=<your_install_dir> make install_shared_lib
sw/airborne/math/pprz_algebra_int.h
+13
View File
@@ -36,6 +36,19 @@
#include "math/pprz_trig_int.h"
#include <stdlib.h>
struct Uint8Vect3 {
uint8_t x;
uint8_t y;
uint8_t z;
};
struct Int8Vect3 {
int8_t x;
int8_t y;
int8_t z;
};
struct Uint16Vect3 {
uint16_t x;
uint16_t y;
sw/airborne/modules/gps/gps_ubx_ucenter.c
+1
View File
@@ -28,6 +28,7 @@
*/
#include "gps_ubx_ucenter.h"
#include "subsystems/gps/gps_ubx.h"
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
sw/airborne/modules/gps/gps_ubx_ucenter.h
+2
View File
@@ -28,6 +28,8 @@
#ifndef GPS_UBX_UCENTER_H
#define GPS_UBX_UCENTER_H
#include "std.h"
/** U-Center Variables */
#define GPS_UBX_UCENTER_CONFIG_STEPS 17
sw/airborne/peripherals/lis302dl.h
+66
View File
@@ -0,0 +1,66 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/lis302dl.h
*
* ST LIS302DL 3-axis accelerometer driver common interface (I2C and SPI).
*/
#ifndef LIS302DL_H
#define LIS302DL_H
/* Include address and register definition */
#include "peripherals/lis302dl_regs.h"
enum Lis302dlConfStatus {
LIS_CONF_UNINIT = 0,
LIS_CONF_WHO_AM_I = 1,
LIS_CONF_WHO_AM_I_OK = 2,
LIS_CONF_REG2 = 3,
LIS_CONF_REG3 = 4,
LIS_CONF_ENABLE = 5,
LIS_CONF_DONE = 6
};
struct Lis302dlConfig {
bool_t int_invert; ///< Invert Interrupt FALSE: active high, TRUE: active low
bool_t spi_3_wire; ///< Set 3-wire SPI mode, if FALSE: 4-wire SPI mode
/** Filtered Data Selection.
* FALSE: internal filter bypassed;
* TRUE: data from internal filter sent to output register */
bool_t filt_data;
enum Lis302dlRanges range; ///< g Range
enum Lis302dlRates rate; ///< Data Output Rate
};
static inline void lis302dl_set_default_config(struct Lis302dlConfig *c)
{
c->int_invert = TRUE;
c->filt_data = FALSE;
c->spi_3_wire = FALSE;
c->rate = LIS302DL_RATE_100HZ;
c->range = LIS302DL_RANGE_2G;
}
#endif /* LIS302DL_H */
sw/airborne/peripherals/lis302dl_regs.h
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/lis302dl_regs.h
*
* ST LIS302DL 3-axis accelerometer register definitions.
*/
#ifndef LIS302DL_REGS_H
#define LIS302DL_REGS_H
/* Registers */
#define LIS302DL_REG_WHO_AM_I 0x0F
#define LIS302DL_REG_CTRL_REG1 0x20
#define LIS302DL_REG_CTRL_REG2 0x21
#define LIS302DL_REG_CTRL_REG3 0x22
#define LIS302DL_REG_STATUS 0x27
#define LIS302DL_REG_OUTX 0x29
#define LIS302DL_REG_OUTY 0x2B
#define LIS302DL_REG_OUTZ 0x2D
/** LIS302DL device identifier contained in LIS302DL_REG_WHO_AM_I */
#define LIS302DL_WHO_AM_I 0x3B
enum Lis302dlRates {
LIS302DL_RATE_100HZ = 0,
LIS302DL_RATE_400HZ = 1
};
enum Lis302dlRanges {
LIS302DL_RANGE_2G = 0,
LIS302DL_RANGE_8G = 1
};
#endif /* LIS302DL_REGS_H */
sw/airborne/peripherals/lis302dl_spi.c
+174
View File
@@ -0,0 +1,174 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/lis302dl_spi.c
*
* Driver for LIS302DL 3-axis accelerometer from ST using SPI.
*/
#include "peripherals/lis302dl_spi.h"
void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t slave_idx)
{
/* set spi_peripheral */
lis->spi_p = spi_p;
/* configure spi transaction */
lis->spi_trans.cpol = SPICpolIdleHigh;
lis->spi_trans.cpha = SPICphaEdge2;
lis->spi_trans.dss = SPIDss8bit;
lis->spi_trans.bitorder = SPIMSBFirst;
lis->spi_trans.cdiv = SPIDiv64;
lis->spi_trans.select = SPISelectUnselect;
lis->spi_trans.slave_idx = slave_idx;
lis->spi_trans.output_length = 2;
lis->spi_trans.input_length = 8;
// callback currently unused
lis->spi_trans.before_cb = NULL;
lis->spi_trans.after_cb = NULL;
lis->spi_trans.input_buf = &(lis->rx_buf[0]);
lis->spi_trans.output_buf = &(lis->tx_buf[0]);
/* set inital status: Success or Done */
lis->spi_trans.status = SPITransDone;
/* set default LIS302DL config options */
lis302dl_set_default_config(&(lis->config));
lis->initialized = FALSE;
lis->data_available = FALSE;
lis->init_status = LIS_CONF_UNINIT;
}
static void lis302dl_spi_write_to_reg(struct Lis302dl_Spi *lis, uint8_t _reg, uint8_t _val) {
lis->spi_trans.output_length = 2;
lis->spi_trans.input_length = 0;
lis->tx_buf[0] = _reg;
lis->tx_buf[1] = _val;
spi_submit(lis->spi_p, &(lis->spi_trans));
}
// Configuration function called once before normal use
static void lis302dl_spi_send_config(struct Lis302dl_Spi *lis)
{
uint8_t reg_val = 0;
switch (lis->init_status) {
case LIS_CONF_WHO_AM_I:
/* query device id */
lis->spi_trans.output_length = 1;
lis->spi_trans.input_length = 2;
/* set read bit then reg address */
lis->tx_buf[0] = (1<<7 | LIS302DL_REG_WHO_AM_I);
if (spi_submit(lis->spi_p, &(lis->spi_trans)))
lis->init_status++;
break;
case LIS_CONF_REG2:
/* set SPI mode, Filtered Data Selection */
reg_val = (lis->config.spi_3_wire << 7) | (lis->config.filt_data << 4);
lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG2, reg_val);
lis->init_status++;
break;
case LIS_CONF_REG3:
/* Interrupt active high/low */
lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG3, (lis->config.int_invert << 7));
lis->init_status++;
break;
case LIS_CONF_ENABLE:
/* set data rate, range, enable measurement, is in standby after power up */
reg_val = (lis->config.rate << 7) |
(1 << 6) | // Power Down Control to active mode
(lis->config.range << 5) |
0x5; // enable z,y,x axes
lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG1, reg_val);
lis->init_status++;
break;
case LIS_CONF_DONE:
lis->initialized = TRUE;
lis->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis)
{
if (lis->init_status == LIS_CONF_UNINIT) {
lis->init_status++;
if (lis->spi_trans.status == SPITransSuccess || lis->spi_trans.status == SPITransDone) {
lis302dl_spi_send_config(lis);
}
}
}
void lis302dl_spi_read(struct Lis302dl_Spi *lis)
{
if (lis->initialized && lis->spi_trans.status == SPITransDone) {
lis->spi_trans.output_length = 1;
lis->spi_trans.input_length = 8;
/* set read bit and multiple byte bit, then address */
lis->tx_buf[0] = (1<<7|1<<6|LIS302DL_REG_STATUS);
spi_submit(lis->spi_p, &(lis->spi_trans));
}
}
void lis302dl_spi_event(struct Lis302dl_Spi *lis)
{
if (lis->initialized) {
if (lis->spi_trans.status == SPITransFailed) {
lis->spi_trans.status = SPITransDone;
}
else if (lis->spi_trans.status == SPITransSuccess) {
// Successfull reading
if (bit_is_set(lis->rx_buf[1], 3)) {
// new xyz data available
lis->data.vect.x = lis->rx_buf[3];
lis->data.vect.y = lis->rx_buf[5];
lis->data.vect.z = lis->rx_buf[7];
lis->data_available = TRUE;
}
lis->spi_trans.status = SPITransDone;
}
}
else if (lis->init_status != LIS_CONF_UNINIT) { // Configuring but not yet initialized
switch (lis->spi_trans.status) {
case SPITransFailed:
lis->init_status--; // Retry config (TODO max retry)
case SPITransSuccess:
if (lis->init_status == LIS_CONF_WHO_AM_I_OK) {
if (lis->rx_buf[1] == LIS302DL_WHO_AM_I)
lis->init_status++;
else
lis->init_status = LIS_CONF_WHO_AM_I;
}
case SPITransDone:
lis->spi_trans.status = SPITransDone;
lis302dl_spi_send_config(lis);
break;
default:
break;
}
}
}
sw/airborne/peripherals/lis302dl_spi.h
+68
View File
@@ -0,0 +1,68 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/lis302dl_spi.h
*
* Driver for LIS302DL 3-axis accelerometer from ST using SPI.
*/
#ifndef LIS302DL_SPI_H
#define LIS302DL_SPI_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "mcu_periph/spi.h"
/* Include common LIS302DL options and definitions */
#include "peripherals/lis302dl.h"
struct Lis302dl_Spi {
struct spi_periph *spi_p;
struct spi_transaction spi_trans;
volatile uint8_t tx_buf[2];
volatile uint8_t rx_buf[8];
enum Lis302dlConfStatus init_status; ///< init status
bool_t initialized; ///< config done flag
volatile bool_t data_available; ///< data ready flag
union {
struct Int8Vect3 vect; ///< data vector in accel coordinate system
int8_t value[3]; ///< data values accessible by channel index
} data;
struct Lis302dlConfig config;
};
// Functions
extern void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t addr);
extern void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis);
extern void lis302dl_spi_read(struct Lis302dl_Spi *lis);
extern void lis302dl_spi_event(struct Lis302dl_Spi *lis);
/// convenience function: read or start configuration if not already initialized
static inline void lis302dl_spi_periodic(struct Lis302dl_Spi *lis) {
if (lis->initialized)
lis302dl_spi_read(lis);
else
lis302dl_spi_start_configure(lis);
}
#endif // LIS302DL_SPI_H
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+19 -9
View File
@@ -50,9 +50,15 @@
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif
#ifndef AHRS_MAG_NOISE_X
#define AHRS_MAG_NOISE_X 0.2
#define AHRS_MAG_NOISE_Y 0.2
#define AHRS_MAG_NOISE_Z 0.2
#endif
static inline void propagate_ref(void);
static inline void propagate_state(void);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
static inline void reset_state(void);
static inline void set_body_state_from_quat(void);
@@ -93,6 +99,8 @@ void ahrs_init(void) {
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
}
void ahrs_align(void) {
@@ -128,9 +136,8 @@ void ahrs_update_accel(void) {
(1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
const struct FloatVect3 earth_g = {0., 0., -9.81 };
const float dn = 250*fabs( ahrs_impl.lp_accel );
const float g_noise[] = {1.+dn, 1.+dn, 1.+dn};
// const float g_noise[] = {150., 150., 150.};
update_state(&earth_g, &imu_g, g_noise);
struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
update_state(&earth_g, &imu_g, &g_noise);
reset_state();
}
@@ -138,8 +145,7 @@ void ahrs_update_accel(void) {
void ahrs_update_mag(void) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
const float h_noise[] = { 0.1610, 0.1771, 0.2659};
update_state(&ahrs_impl.mag_h, &imu_h, h_noise);
update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
reset_state();
}
@@ -200,7 +206,7 @@ static inline void propagate_state(void) {
/**
* Incorporate one 3D vector measurement
*/
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]) {
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
@@ -214,8 +220,12 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
float S[3][3];
MAT_MUL_T(3,6,3, S, tmp, H);
for (int i=0;i<3;i++)
S[i][i] += noise[i];
/* add the measurement noise */
S[0][0] += noise->x;
S[1][1] += noise->y;
S[2][2] += noise->z;
float invS[3][3];
MAT_INV33(invS, S);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+2
View File
@@ -43,6 +43,8 @@ struct AhrsMlkf {
struct FloatVect3 mag_h;
struct FloatVect3 mag_noise;
struct FloatQuat gibbs_cor;
float P[6][6];
float lp_accel;
sw/airborne/subsystems/gps/gps_ubx.c
-28
View File
@@ -53,30 +53,7 @@
#define UTM_HEM_SOUTH 1
#define GpsUartSend1(c) GpsLink(Transmit(c))
#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
#define GpsUartRunning GpsLink(TxRunning)
#define GpsUartSendMessage GpsLink(SendMessage)
#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; }
#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; }
#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); }
#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
#define UbxSend1ByAddr(x) { UbxSend1(*x); }
#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
#define UbxHeader(nav_id, msg_id, len) { \
GpsUartSend1(UBX_SYNC1); \
GpsUartSend1(UBX_SYNC2); \
UbxInitCheksum(); \
UbxSend1(nav_id); \
UbxSend1(msg_id); \
UbxSend2(len); \
}
struct GpsUbx gps_ubx;
@@ -267,11 +244,6 @@ void gps_ubx_parse( uint8_t c ) {
return;
}
#ifdef GPS_UBX_UCENTER
#include GPS_UBX_UCENTER
#endif
void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
#ifdef GPS_LINK
UbxSend_CFG_RST(bbr, reset_mode, 0x00);
sw/airborne/subsystems/gps/gps_ubx.h
+22
View File
@@ -134,5 +134,27 @@ extern void ubxsend_cfg_rst(uint16_t, uint8_t);
ubxsend_cfg_rst(gps_ubx.reset, CFG_RST_Reset_Controlled); \
}
#define GpsUartSendMessage GpsLink(SendMessage)
#define GpsUartSend1(c) GpsLink(Transmit(c))
#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; }
#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; }
#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
#define UbxSend1ByAddr(x) { UbxSend1(*x); }
#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
#define UbxHeader(nav_id, msg_id, len) { \
GpsUartSend1(UBX_SYNC1); \
GpsUartSend1(UBX_SYNC2); \
UbxInitCheksum(); \
UbxSend1(nav_id); \
UbxSend1(msg_id); \
UbxSend2(len); \
}
#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); }
#endif /* GPS_UBX_H */
sw/airborne/test/Makefile
+1 -13
View File
@@ -18,23 +18,11 @@ test_geodetic: test_geodetic.c ../math/pprz_geodetic_float.c ../math/pprz_geodet
test_algebra: test_algebra.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_martin: test_martin.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_bla: test_bla.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_att: test_att.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_sqrt: test_sqrt.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_fmul: test_fmul.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
%.exe : %.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean:
$(Q)rm -f *~ test_geodetic test_algebra *.exe
$(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe
sw/airborne/test/ahrs/ahrs_utils.py
+65 -72
View File
@@ -1,6 +1,5 @@
#! /usr/bin/env python
# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# This file is part of Paparazzi.
@@ -21,37 +20,32 @@
# Boston, MA 02111-1307, USA.
#
#import os
#from optparse import OptionParser
#import scipy
#from scipy import optimize
import shlex, subprocess
from pylab import *
from array import array
import numpy
from __future__ import print_function
import subprocess
import numpy as np
import matplotlib.pyplot as plt
def run_simulation(ahrs_type, build_opt, traj_nb):
print "\nBuilding ahrs"
print("\nBuilding ahrs")
args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt
# print args
#print(args)
p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
outputlines = p.stdout.readlines()
p.wait()
for i in outputlines:
print " # " + i,
print
print "Running simulation"
print " using traj " + str(traj_nb)
print(" # " + i, end=' ')
print()
print("Running simulation")
print(" using traj " + str(traj_nb))
p = subprocess.Popen(args=["./run_ahrs_on_synth", str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
shell=False)
outputlines = p.stdout.readlines()
p.wait()
# for i in outputlines:
# print " "+i,
# print "\n"
# print(" "+i, end=' ')
# print("\n")
ahrs_data_type = [('time', 'float32'),
('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
@@ -61,92 +55,91 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')]
mydescr = numpy.dtype(ahrs_data_type)
mydescr = np.dtype(ahrs_data_type)
data = [[] for dummy in xrange(len(mydescr))]
# import code; code.interact(local=locals())
for line in outputlines:
if line.startswith("#"):
print " " + line,
print(" " + line, end=' ')
else:
fields = line.strip().split(' ')
# print fields
#print(fields)
for i, number in enumerate(fields):
data[i].append(number)
print
print()
for i in xrange(len(mydescr)):
data[i] = cast[mydescr[i]](data[i])
data[i] = np.cast[mydescr[i]](data[i])
return numpy.rec.array(data, dtype=mydescr)
return np.rec.array(data, dtype=mydescr)
def plot_simulation_results(plot_true_state, lsty, type, sim_res):
print "Plotting Results"
def plot_simulation_results(plot_true_state, lsty, label, sim_res):
print("Plotting Results")
# f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True)
subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type)
ylabel('degres')
title('phi')
legend()
plt.subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=label)
plt.ylabel('degres')
plt.title('phi')
plt.legend()
subplot(3, 3, 2)
plot(sim_res.time, sim_res.theta_ahrs, lsty)
title('theta')
plt.subplot(3, 3, 2)
plt.plot(sim_res.time, sim_res.theta_ahrs, lsty)
plt.title('theta')
subplot(3, 3, 3)
plot(sim_res.time, sim_res.psi_ahrs, lsty)
title('psi')
plt.subplot(3, 3, 3)
plt.plot(sim_res.time, sim_res.psi_ahrs, lsty)
plt.title('psi')
subplot(3, 3, 4)
plt.subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
ylabel('degres/s')
title('p')
plt.ylabel('degres/s')
plt.title('p')
subplot(3, 3, 5)
plt.subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
title('q')
plt.title('q')
subplot(3, 3, 6)
plt.subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
title('r')
plt.title('r')
subplot(3, 3, 7)
plt.subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
ylabel('degres/s')
xlabel('time in s')
title('bp')
plt.ylabel('degres/s')
plt.xlabel('time in s')
plt.title('bp')
subplot(3, 3, 8)
plt.subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
xlabel('time in s')
title('bq')
plt.xlabel('time in s')
plt.title('bq')
subplot(3, 3, 9)
plt.subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
xlabel('time in s')
title('br')
plt.xlabel('time in s')
plt.title('br')
if plot_true_state:
subplot(3, 3, 1)
plt.subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_true, 'r--')
subplot(3, 3, 2)
plot(sim_res.time, sim_res.theta_true, 'r--')
subplot(3, 3, 3)
plot(sim_res.time, sim_res.psi_true, 'r--')
subplot(3, 3, 4)
plot(sim_res.time, sim_res.p_true, 'r--')
subplot(3, 3, 5)
plot(sim_res.time, sim_res.q_true, 'r--')
subplot(3, 3, 6)
plot(sim_res.time, sim_res.r_true, 'r--')
subplot(3, 3, 7)
plot(sim_res.time, sim_res.bp_true, 'r--')
subplot(3, 3, 8)
plot(sim_res.time, sim_res.bq_true, 'r--')
subplot(3, 3, 9)
plot(sim_res.time, sim_res.br_true, 'r--')
plt.subplot(3, 3, 2)
plt.plot(sim_res.time, sim_res.theta_true, 'r--')
plt.subplot(3, 3, 3)
plt.plot(sim_res.time, sim_res.psi_true, 'r--')
plt.subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_true, 'r--')
plt.subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_true, 'r--')
plt.subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_true, 'r--')
plt.subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_true, 'r--')
plt.subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_true, 'r--')
plt.subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_true, 'r--')
def show_plot():
sw/airborne/test/ahrs/compare_ahrs.py
+6
View File
@@ -20,8 +20,11 @@
# Boston, MA 02111-1307, USA.
#
import os
import sys
import ahrs_utils
def main():
# traj_nb = 0 # static
@@ -77,4 +80,7 @@ def main():
ahrs_utils.show_plot()
if __name__ == "__main__":
script_path = os.path.dirname(os.path.realpath(sys.argv[0]))
if script_path != os.path.abspath(os.getcwd()):
sys.exit("Please run this script from " + script_path)
main()
sw/airborne/test/nova_test_imu.c
-58
View File
@@ -1,58 +0,0 @@
#include "std.h"
#include "mcu.h"
#include "interrupt_hw.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "ADS8344.h"
static inline void main_init( void );
static inline void main_periodic( void );
static inline void main_event( void );
int main( void ) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic();
main_event();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
uart1_init_tx();
ADS8344_init();
mcu_int_enable();
ADS8344_start();
}
static inline void main_periodic( void ) {
DOWNLINK_SEND_BOOT(&sys_time.nb_sec);
}
static inline void main_event( void ) {
if ( ADS8344_available ) {
float foo = ADS8344_values[0];
float foo1 = ADS8344_values[1];
float foo2 = ADS8344_values[2];
DOWNLINK_SEND_IMU_GYRO(&foo, &foo1, &foo2);
// LED_TOGGLE(2);
ADS8344_available = FALSE;
}
}
sw/airborne/booz/test/booz2_test_usb.c → sw/airborne/test/peripherals/test_lis302dl_spi.c
+50 -18
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -19,53 +19,85 @@
* Boston, MA 02111-1307, USA.
*/
#include <inttypes.h>
/**
* @file test/peripherals/test_lis302dl_spi.c
*
* Test for LIS302DL 3-axis accelerometer from ST using SPI.
*/
#include "std.h"
#include BOARD_CONFIG
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "led.h"
#include "mcu_periph/usb_serial.h"
#include "peripherals/lis302dl_spi.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#ifndef LIS302DL_SPI_DEV
#define LIS302DL_SPI_DEV spi2
#endif
PRINT_CONFIG_VAR(LIS302DL_SPI_DEV)
#include "interrupt_hw.h"
#ifndef LIS302DL_SPI_SLAVE_IDX
#define LIS302DL_SPI_SLAVE_IDX SPI_SLAVE2
#endif
PRINT_CONFIG_VAR(LIS302DL_SPI_SLAVE_IDX)
struct Lis302dl_Spi lis302;
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
int main( void ) {
int main(void) {
main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
VCOM_init();
mcu_int_enable();
sys_time_register_timer((1./50), NULL);
lis302dl_spi_init(&lis302, &(LIS302DL_SPI_DEV), LIS302DL_SPI_SLAVE_IDX);
}
static inline void main_periodic_task( void ) {
RunOnceEvery(10, {
LED_TOGGLE(1);
DOWNLINK_SEND_ALIVE(PprzTransport,16, MD5SUM);
});
RunOnceEvery(100, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
if (sys_time.nb_sec > 1) {
lis302dl_spi_periodic(&lis302);
#if USE_LED_5
RunOnceEvery(10, LED_TOGGLE(5););
#endif
}
}
static inline void main_event_task( void ) {
if (sys_time.nb_sec > 1)
lis302dl_spi_event(&lis302);
if (lis302.data_available) {
struct Int32Vect3 accel;
VECT3_COPY(accel, lis302.data.vect);
lis302.data_available = FALSE;
RunOnceEvery(10, {
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
&accel.x, &accel.y, &accel.z);
#if USE_LED_6
LED_TOGGLE(6);
#endif
});
}
}
sw/airborne/test/stabilization/.gitignore
+6
View File
@@ -0,0 +1,6 @@
*.so
algebra_int.c
algebra_float.c
ref_quat_float.c
ref_quat_int.c
build
sw/airborne/test/stabilization/README.md
+11
View File
@@ -0,0 +1,11 @@
Some Cython wrappers for stabilization reference generation testing.
Tested with Cython 0.19
To build execute in this directory:
python setup.py build_ext --inplace
Run the example script to compare float and int ref quat implementations:
./compare_ref_quat.py
sw/airborne/test/stabilization/algebra_float.pxd
+10
View File
@@ -0,0 +1,10 @@
cimport pprz_algebra_float_c
cdef class FloatEulers:
cdef pprz_algebra_float_c.FloatEulers *ptr
cdef class FloatQuat:
cdef pprz_algebra_float_c.FloatQuat *ptr
cdef class FloatRates:
cdef pprz_algebra_float_c.FloatRates *ptr
sw/airborne/test/stabilization/algebra_float.pyx
+118
View File
@@ -0,0 +1,118 @@
cdef extern from "stdint.h":
ctypedef unsigned long int uintptr_t
cimport pprz_algebra_float_c
cimport algebra_float
cimport numpy as np
import numpy as np
cdef class FloatRates:
# already declared in pxd
#cdef pprz_algebra_float_c.FloatRates *ptr
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_float_c.FloatRates*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
def __set__(self, np.ndarray a):
self.ptr[0].p = a[0]
self.ptr[0].q = a[1]
self.ptr[0].r = a[2]
property p:
def __get__(self):
return self.ptr[0].p
def __set__(self, v):
self.ptr[0].p = v
property q:
def __get__(self):
return self.ptr[0].q
def __set__(self, v):
self.ptr[0].q = v
property r:
def __get__(self):
return self.ptr[0].r
def __set__(self, v):
self.ptr[0].r = v
cdef class FloatEulers:
# already declared in pxd
#cdef pprz_algebra_float_c.FloatEulers *ptr
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_float_c.FloatEulers*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
self.ptr[0].psi])
def __set__(self, np.ndarray a):
self.ptr[0].phi = a[0]
self.ptr[0].theta = a[1]
self.ptr[0].psi = a[2]
property phi:
def __get__(self):
return self.ptr[0].phi
def __set__(self, v):
self.ptr[0].phi = v
property theta:
def __get__(self):
return self.ptr[0].theta
def __set__(self, v):
self.ptr[0].theta = v
property psi:
def __get__(self):
return self.ptr[0].psi
def __set__(self, v):
self.ptr[0].psi = v
cdef class FloatQuat:
# already declared in pxd
#cdef pprz_algebra_float_c.FloatQuat *ptr
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_float_c.FloatQuat*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
self.ptr[0].qy, self.ptr[0].qz])
def __set__(self, np.ndarray a):
self.ptr[0].qi = a[0]
self.ptr[0].qx = a[1]
self.ptr[0].qy = a[2]
self.ptr[0].qz = a[3]
property qi:
def __get__(self):
return self.ptr[0].qi
def __set__(self, v):
self.ptr[0].qi = v
property qx:
def __get__(self):
return self.ptr[0].qx
def __set__(self, v):
self.ptr[0].qx = v
property qy:
def __get__(self):
return self.ptr[0].qy
def __set__(self, v):
self.ptr[0].qy = v
property qz:
def __get__(self):
return self.ptr[0].qz
def __set__(self, v):
self.ptr[0].qz = v
sw/airborne/test/stabilization/algebra_int.pxd
+10
View File
@@ -0,0 +1,10 @@
cimport pprz_algebra_int_c
cdef class Int32Eulers:
cdef pprz_algebra_int_c.Int32Eulers *ptr
cdef class Int32Quat:
cdef pprz_algebra_int_c.Int32Quat *ptr
cdef class Int32Rates:
cdef pprz_algebra_int_c.Int32Rates *ptr
sw/airborne/test/stabilization/algebra_int.pyx
+112
View File
@@ -0,0 +1,112 @@
cdef extern from "stdint.h":
ctypedef unsigned long int uintptr_t
cimport pprz_algebra_int_c
cimport algebra_int
cimport numpy as np
import numpy as np
cdef class Int32Rates:
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_int_c.Int32Rates*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
def __set__(self, np.ndarray a):
self.ptr[0].p = a[0]
self.ptr[0].q = a[1]
self.ptr[0].r = a[2]
property p:
def __get__(self):
return self.ptr[0].p
def __set__(self, v):
self.ptr[0].p = v
property q:
def __get__(self):
return self.ptr[0].q
def __set__(self, v):
self.ptr[0].q = v
property r:
def __get__(self):
return self.ptr[0].r
def __set__(self, v):
self.ptr[0].r = v
cdef class Int32Eulers:
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_int_c.Int32Eulers*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
self.ptr[0].psi])
def __set__(self, np.ndarray a):
self.ptr[0].phi = a[0]
self.ptr[0].theta = a[1]
self.ptr[0].psi = a[2]
property phi:
def __get__(self):
return self.ptr[0].phi
def __set__(self, v):
self.ptr[0].phi = v
property theta:
def __get__(self):
return self.ptr[0].theta
def __set__(self, v):
self.ptr[0].theta = v
property psi:
def __get__(self):
return self.ptr[0].psi
def __set__(self, v):
self.ptr[0].psi = v
cdef class Int32Quat:
def __cinit__(self, uintptr_t p):
self.ptr = <pprz_algebra_int_c.Int32Quat*> p
property array:
def __get__(self):
return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
self.ptr[0].qy, self.ptr[0].qz])
def __set__(self, np.ndarray a):
self.ptr[0].qi = a[0]
self.ptr[0].qx = a[1]
self.ptr[0].qy = a[2]
self.ptr[0].qz = a[3]
property qi:
def __get__(self):
return self.ptr[0].qi
def __set__(self, v):
self.ptr[0].qi = v
property qx:
def __get__(self):
return self.ptr[0].qx
def __set__(self, v):
self.ptr[0].qx = v
property qy:
def __get__(self):
return self.ptr[0].qy
def __set__(self, v):
self.ptr[0].qy = v
property qz:
def __get__(self):
return self.ptr[0].qz
def __set__(self, v):
self.ptr[0].qz = v
sw/airborne/test/stabilization/compare_ref_quat.py
Executable
+50
View File
@@ -0,0 +1,50 @@
#! /usr/bin/env python
from __future__ import division, print_function, absolute_import
import numpy as np
import matplotlib.pyplot as plt
import ref_quat_float
import ref_quat_int
steps = 512 * 2
ref_eul_res = np.zeros((steps, 3))
ref_quat_res = np.zeros((steps, 3))
ref_quat_float.init()
ref_quat_int.init()
# reset psi and update_ref_quat_from_eulers
ref_quat_float.enter()
ref_quat_int.enter()
q_sp = np.array([0.92387956, 0.38268346, 0., 0.])
ref_quat_float.sp_quat.array = q_sp
ref_quat_int.sp_quat.array = q_sp * (1 << 15)
for i in range(0, steps):
ref_quat_float.update()
ref_eul_res[i, :] = ref_quat_float.ref_euler.array
ref_quat_int.update()
ref_quat_res[i, :] = ref_quat_int.ref_euler.array / (1 << 20)
plt.figure(1)
plt.subplot(311)
plt.title("reference in euler angles")
plt.plot(np.degrees(ref_eul_res[:, 0]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 0]), 'r')
plt.ylabel("phi [deg]")
plt.subplot(312)
plt.plot(np.degrees(ref_eul_res[:, 1]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 1]), 'r')
plt.ylabel("theta [deg]")
plt.subplot(313)
plt.plot(np.degrees(ref_eul_res[:, 2]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 2]), 'r')
plt.ylabel("psi [deg]")
plt.show()
sw/airborne/test/stabilization/generated/airframe.h
+51
View File
@@ -0,0 +1,51 @@
/* fake generated airframe file */
#ifndef AIRFRAME_H
#define AIRFRAME_H
#define PERIODIC_FREQUENCY 512
#if defined STABILIZATION_ATTITUDE_TYPE_FLOAT
#define SECTION_STABILIZATION_ATTITUDE 1
#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(800)}
#define STABILIZATION_ATTITUDE_REF_ZETA_P {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(800)}
#define STABILIZATION_ATTITUDE_REF_ZETA_Q {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(500)}
#define STABILIZATION_ATTITUDE_REF_ZETA_R {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
#elif defined STABILIZATION_ATTITUDE_TYPE_INT
#define SECTION_STABILIZATION_ATTITUDE 1
#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
#define STABILIZATION_ATTITUDE_REF_OMEGA_P 13.962634
#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 13.962634
#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R 8.72664625
#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
#endif
#endif // AIRFRAME_H
sw/airborne/test/stabilization/pprz_algebra_double_c.pxd
+29
View File
@@ -0,0 +1,29 @@
cdef extern from "math/pprz_algebra_double.h":
struct DoubleVect2
double x
double y
struct DoubleVect3
double x
double y
double z
struct DoubleEulers:
double phi
double theta
double psi
struct DoubleQuat:
double qi
double qx
double qy
double qz
struct DoubleRates:
double p
double q
double r
struct DoubleRMat:
double m[3*3]
sw/airborne/test/stabilization/pprz_algebra_float_c.pxd
+29
View File
@@ -0,0 +1,29 @@
cdef extern from "math/pprz_algebra_float.h":
struct FloatVect2:
float x
float y
struct FloatVect3:
float x
float y
float z
struct FloatEulers:
float phi
float theta
float psi
struct FloatQuat:
float qi
float qx
float qy
float qz
struct FloatRates:
float p
float q
float r
struct FloatRMat:
float m[3*3]

Some files were not shown because too many files have changed in this diff Show More