forked from Archive/PX4-Autopilot
Merged
This commit is contained in:
Lorenz Meier
commit
bbecaa7de3
2
Makefile
2
Makefile
|
|
@ -174,7 +174,7 @@ clean:
|
|||
distclean: clean
|
||||
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
|
||||
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
|
||||
$(Q) (cd $(NUTTX_SRC)/configs && find . -type l -depth 1 -delete)
|
||||
$(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete)
|
||||
|
||||
#
|
||||
# Print some help text
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ then
|
|||
else
|
||||
echo "using L3GD20 and LSM303D"
|
||||
l3gd20 start
|
||||
lsm303 start
|
||||
lsm303d start
|
||||
fi
|
||||
|
||||
#
|
||||
|
|
@ -40,4 +40,4 @@ then
|
|||
# Check sensors - run AFTER 'sensors start'
|
||||
#
|
||||
preflight_check &
|
||||
fi
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@ MODULES += drivers/led
|
|||
MODULES += drivers/px4io
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/px4fmu
|
||||
MODULES += drivers/lsm303d
|
||||
MODULES += drivers/ardrone_interface
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/bma180
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@ MODULES += drivers/device
|
|||
MODULES += drivers/stm32
|
||||
MODULES += drivers/stm32/adc
|
||||
MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/px4io
|
||||
MODULES += drivers/boards/px4fmuv2
|
||||
|
|
|
|||
|
|
@ -242,6 +242,9 @@ CONFIG_STM32_ADC=y
|
|||
CONFIG_STM32_SPI=y
|
||||
CONFIG_STM32_I2C=y
|
||||
|
||||
CONFIG_ARCH_HAVE_LEDS=y
|
||||
# CONFIG_ARCH_LEDS is not set
|
||||
|
||||
#
|
||||
# Alternate Pin Mapping
|
||||
#
|
||||
|
|
@ -253,7 +256,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y
|
|||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
# CONFIG_STM32_FORCEPOWER is not set
|
||||
# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
|
||||
# CONFIG_STM32_CCMEXCLUDE is not set
|
||||
CONFIG_STM32_CCMEXCLUDE=y
|
||||
CONFIG_STM32_DMACAPABLE=y
|
||||
# CONFIG_STM32_TIM1_PWM is not set
|
||||
# CONFIG_STM32_TIM3_PWM is not set
|
||||
|
|
@ -274,9 +277,9 @@ CONFIG_USART1_RXDMA=y
|
|||
# CONFIG_USART2_RS485 is not set
|
||||
CONFIG_USART2_RXDMA=y
|
||||
# CONFIG_USART3_RS485 is not set
|
||||
CONFIG_USART3_RXDMA=y
|
||||
# CONFIG_USART3_RXDMA is not set
|
||||
# CONFIG_UART4_RS485 is not set
|
||||
CONFIG_UART4_RXDMA=y
|
||||
# CONFIG_UART4_RXDMA is not set
|
||||
# CONFIG_UART5_RXDMA is not set
|
||||
# CONFIG_USART6_RS485 is not set
|
||||
# CONFIG_USART6_RXDMA is not set
|
||||
|
|
@ -284,6 +287,26 @@ CONFIG_UART4_RXDMA=y
|
|||
CONFIG_USART8_RXDMA=y
|
||||
CONFIG_STM32_USART_SINGLEWIRE=y
|
||||
|
||||
|
||||
# Previous:
|
||||
## CONFIG_USART1_RS485 is not set
|
||||
#CONFIG_USART1_RXDMA=y
|
||||
## CONFIG_USART2_RS485 is not set
|
||||
#CONFIG_USART2_RXDMA=y
|
||||
## CONFIG_USART3_RS485 is not set
|
||||
#CONFIG_USART3_RXDMA=y
|
||||
## CONFIG_UART4_RS485 is not set
|
||||
#CONFIG_UART4_RXDMA=y
|
||||
## CONFIG_UART5_RXDMA is not set
|
||||
## CONFIG_USART6_RS485 is not set
|
||||
## CONFIG_USART6_RXDMA is not set
|
||||
## CONFIG_USART7_RXDMA is not set
|
||||
#CONFIG_USART8_RXDMA=y
|
||||
#CONFIG_STM32_USART_SINGLEWIRE=y
|
||||
|
||||
|
||||
|
||||
|
||||
#
|
||||
# SPI Configuration
|
||||
#
|
||||
|
|
@ -302,7 +325,23 @@ CONFIG_STM32_I2CTIMEOTICKS=500
|
|||
#
|
||||
# SDIO Configuration
|
||||
#
|
||||
CONFIG_SDIO_PRI=128
|
||||
|
||||
#
|
||||
# Maintain legacy build behavior (revisit)
|
||||
#
|
||||
|
||||
CONFIG_MMCSD=y
|
||||
#CONFIG_MMCSD_SPI=y
|
||||
CONFIG_MMCSD_SDIO=y
|
||||
CONFIG_MTD=y
|
||||
|
||||
#
|
||||
# STM32 SDIO-based MMC/SD driver
|
||||
#
|
||||
CONFIG_SDIO_DMA=y
|
||||
CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
|
||||
CONFIG_MMCSD_MMCSUPPORT=n
|
||||
CONFIG_MMCSD_HAVECARDDETECT=n
|
||||
|
||||
#
|
||||
# USB Host Configuration
|
||||
|
|
@ -361,6 +400,7 @@ CONFIG_ARCH_BOARD="px4fmu-v2"
|
|||
#
|
||||
# Common Board Options
|
||||
#
|
||||
CONFIG_NSH_MMCSDSLOTNO=0
|
||||
CONFIG_NSH_MMCSDMINOR=0
|
||||
|
||||
#
|
||||
|
|
@ -508,8 +548,8 @@ CONFIG_NO_SERIAL_CONSOLE=y
|
|||
#
|
||||
# USART1 Configuration
|
||||
#
|
||||
CONFIG_USART1_RXBUFSIZE=16
|
||||
CONFIG_USART1_TXBUFSIZE=16
|
||||
CONFIG_USART1_RXBUFSIZE=512
|
||||
CONFIG_USART1_TXBUFSIZE=512
|
||||
CONFIG_USART1_BAUD=115200
|
||||
CONFIG_USART1_BITS=8
|
||||
CONFIG_USART1_PARITY=0
|
||||
|
|
@ -534,7 +574,7 @@ CONFIG_USART2_OFLOWCONTROL=y
|
|||
#
|
||||
CONFIG_USART3_RXBUFSIZE=512
|
||||
CONFIG_USART3_TXBUFSIZE=512
|
||||
CONFIG_USART3_BAUD=115200
|
||||
CONFIG_USART3_BAUD=57600
|
||||
CONFIG_USART3_BITS=8
|
||||
CONFIG_USART3_PARITY=0
|
||||
CONFIG_USART3_2STOP=0
|
||||
|
|
@ -556,9 +596,9 @@ CONFIG_UART4_2STOP=0
|
|||
#
|
||||
# USART6 Configuration
|
||||
#
|
||||
CONFIG_USART6_RXBUFSIZE=16
|
||||
CONFIG_USART6_TXBUFSIZE=16
|
||||
CONFIG_USART6_BAUD=115200
|
||||
CONFIG_USART6_RXBUFSIZE=256
|
||||
CONFIG_USART6_TXBUFSIZE=256
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_BITS=8
|
||||
CONFIG_USART6_PARITY=0
|
||||
CONFIG_USART6_2STOP=0
|
||||
|
|
@ -568,8 +608,8 @@ CONFIG_USART6_2STOP=0
|
|||
#
|
||||
# UART7 Configuration
|
||||
#
|
||||
CONFIG_UART7_RXBUFSIZE=128
|
||||
CONFIG_UART7_TXBUFSIZE=128
|
||||
CONFIG_UART7_RXBUFSIZE=256
|
||||
CONFIG_UART7_TXBUFSIZE=256
|
||||
CONFIG_UART7_BAUD=57600
|
||||
CONFIG_UART7_BITS=8
|
||||
CONFIG_UART7_PARITY=0
|
||||
|
|
@ -580,8 +620,8 @@ CONFIG_UART7_2STOP=0
|
|||
#
|
||||
# UART8 Configuration
|
||||
#
|
||||
CONFIG_UART8_RXBUFSIZE=128
|
||||
CONFIG_UART8_TXBUFSIZE=128
|
||||
CONFIG_UART8_RXBUFSIZE=256
|
||||
CONFIG_UART8_TXBUFSIZE=256
|
||||
CONFIG_UART8_BAUD=57600
|
||||
CONFIG_UART8_BITS=8
|
||||
CONFIG_UART8_PARITY=0
|
||||
|
|
|
|||
|
|
@ -195,6 +195,28 @@ SERIAL_HAVE_CONSOLE_DMA=y
|
|||
CONFIG_USART2_RXDMA=n
|
||||
CONFIG_USART3_RXDMA=y
|
||||
|
||||
#
|
||||
# PX4IO specific driver settings
|
||||
#
|
||||
# CONFIG_HRT_TIMER
|
||||
# Enables the high-resolution timer. The board definition must
|
||||
# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
|
||||
# compare channels to be used.
|
||||
# CONFIG_HRT_PPM
|
||||
# Enables R/C PPM input using the HRT. The board definition must
|
||||
# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
|
||||
# used, and define GPIO_PPM_IN to configure the appropriate timer
|
||||
# GPIO.
|
||||
# CONFIG_PWM_SERVO
|
||||
# Enables the PWM servo driver. The driver configuration must be
|
||||
# supplied by the board support at initialisation time.
|
||||
# Note that USART2 must be disabled on the PX4 board for this to
|
||||
# be available.
|
||||
#
|
||||
#
|
||||
CONFIG_HRT_TIMER=y
|
||||
CONFIG_HRT_PPM=y
|
||||
|
||||
#
|
||||
# General build options
|
||||
#
|
||||
|
|
|
|||
|
|
@ -187,6 +187,28 @@ CONFIG_USART1_2STOP=0
|
|||
CONFIG_USART2_2STOP=0
|
||||
CONFIG_USART3_2STOP=0
|
||||
|
||||
#
|
||||
# PX4IO specific driver settings
|
||||
#
|
||||
# CONFIG_HRT_TIMER
|
||||
# Enables the high-resolution timer. The board definition must
|
||||
# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
|
||||
# compare channels to be used.
|
||||
# CONFIG_HRT_PPM
|
||||
# Enables R/C PPM input using the HRT. The board definition must
|
||||
# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
|
||||
# used, and define GPIO_PPM_IN to configure the appropriate timer
|
||||
# GPIO.
|
||||
# CONFIG_PWM_SERVO
|
||||
# Enables the PWM servo driver. The driver configuration must be
|
||||
# supplied by the board support at initialisation time.
|
||||
# Note that USART2 must be disabled on the PX4 board for this to
|
||||
# be available.
|
||||
#
|
||||
#
|
||||
CONFIG_HRT_TIMER=y
|
||||
CONFIG_HRT_PPM=y
|
||||
|
||||
#
|
||||
# General build options
|
||||
#
|
||||
|
|
|
|||
|
|
@ -3,7 +3,8 @@
|
|||
#
|
||||
|
||||
SRCS = px4fmu_can.c \
|
||||
px4fmu_init.c \
|
||||
px4fmu2_init.c \
|
||||
px4fmu_pwm_servo.c \
|
||||
px4fmu_spi.c \
|
||||
px4fmu_usb.c
|
||||
px4fmu_usb.c \
|
||||
px4fmu2_led.c
|
||||
|
|
|
|||
|
|
@ -192,9 +192,8 @@ __EXPORT int nsh_archinitialize(void)
|
|||
(hrt_callout)stm32_serial_dma_poll,
|
||||
NULL);
|
||||
|
||||
// initial LED state
|
||||
// XXX need to make this work again
|
||||
// drv_led_start();
|
||||
/* initial LED state */
|
||||
//drv_led_start();
|
||||
up_ledoff(LED_AMBER);
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4fmu2_led.c
|
||||
*
|
||||
* PX4FMU LED backend.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "stm32.h"
|
||||
#include "px4fmu_internal.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/*
|
||||
* Ideally we'd be able to get these from up_internal.h,
|
||||
* but since we want to be able to disable the NuttX use
|
||||
* of leds for system indication at will and there is no
|
||||
* separate switch, we need to build independent of the
|
||||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init();
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
__END_DECLS
|
||||
|
||||
__EXPORT void led_init()
|
||||
{
|
||||
/* Configure LED1 GPIO for output */
|
||||
|
||||
stm32_configgpio(GPIO_LED1);
|
||||
}
|
||||
|
||||
__EXPORT void led_on(int led)
|
||||
{
|
||||
if (led == 0)
|
||||
{
|
||||
/* Pull down to switch on */
|
||||
stm32_gpiowrite(GPIO_LED1, false);
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void led_off(int led)
|
||||
{
|
||||
if (led == 0)
|
||||
{
|
||||
/* Pull up to switch off */
|
||||
stm32_gpiowrite(GPIO_LED1, true);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -110,7 +110,7 @@ LED *gLED;
|
|||
}
|
||||
|
||||
void
|
||||
drv_led_start()
|
||||
drv_led_start(void)
|
||||
{
|
||||
if (gLED == nullptr) {
|
||||
gLED = new LED;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,255 @@
|
|||
#include "math.h"
|
||||
#include "string.h"
|
||||
#include "iirFilter.h"
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Internal function prototypes
|
||||
|
||||
int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd);
|
||||
|
||||
int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd);
|
||||
|
||||
int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt);
|
||||
|
||||
int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly);
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// external functions
|
||||
|
||||
int testFunction()
|
||||
{
|
||||
printf("TEST\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt)
|
||||
{
|
||||
TF_POLY_t polyd;
|
||||
TF_ZPG_t zpgd;
|
||||
|
||||
memset(pFilt, 0, sizeof(FIL_T));
|
||||
|
||||
// perform bilinear transform with frequency pre warping
|
||||
btDifcToZpgd(pDifc, Ts, &zpgd);
|
||||
|
||||
// calculate polynom
|
||||
tZpgxToPolyx(&zpgd, &polyd);
|
||||
|
||||
// set the filter parameters
|
||||
tPolydToFil(&polyd, pFilt);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
// run filter using inp, return output of the filter
|
||||
float updateFilter(FIL_T *pFilt, float inp)
|
||||
{
|
||||
float outp = 0;
|
||||
int idx; // index used for different things
|
||||
int i; // loop variable
|
||||
|
||||
// Store the input to the input array
|
||||
idx = pFilt->inpCnt % MAX_LENGTH;
|
||||
pFilt->inpData[idx] = inp;
|
||||
|
||||
// calculate numData * inpData
|
||||
for (i = 0; i < pFilt->numLength; i++)
|
||||
{
|
||||
// index of inp array
|
||||
idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH;
|
||||
outp += pFilt->numData[i] * pFilt->inpData[idx];
|
||||
}
|
||||
|
||||
// calculate denData * outData
|
||||
for (i = 0; i < pFilt->denLength; i++)
|
||||
{
|
||||
// index of inp array
|
||||
idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH;
|
||||
outp -= pFilt->denData[i] * pFilt->outData[idx];
|
||||
}
|
||||
|
||||
// store the ouput data to the output array
|
||||
idx = pFilt->inpCnt % MAX_LENGTH;
|
||||
pFilt->outData[idx] = outp;
|
||||
|
||||
pFilt->inpCnt += 1;
|
||||
|
||||
return outp;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Internal functions
|
||||
|
||||
int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt)
|
||||
{
|
||||
double gain;
|
||||
int i;
|
||||
|
||||
if (pkPolyd->Ts < 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// initialize filter to 0
|
||||
memset(pFilt, 0, sizeof(FIL_T));
|
||||
|
||||
gain = pkPolyd->denData[pkPolyd->denLength - 1];
|
||||
pFilt->Ts = pkPolyd->Ts;
|
||||
|
||||
pFilt->denLength = pkPolyd->denLength - 1;
|
||||
pFilt->numLength = pkPolyd->numLength;
|
||||
|
||||
for (i = 0; i < pkPolyd->numLength; i++)
|
||||
{
|
||||
pFilt->numData[i] = pkPolyd->numData[i] / gain;
|
||||
}
|
||||
|
||||
for (i = 0; i < (pkPolyd->denLength - 1); i++)
|
||||
{
|
||||
pFilt->denData[i] = pkPolyd->denData[i] / gain;
|
||||
}
|
||||
}
|
||||
|
||||
// bilinear transformation of poles and zeros
|
||||
int btDifcToZpgd(const TF_DIF_t *pkDifc,
|
||||
double Ts,
|
||||
TF_ZPG_t *pZpgd)
|
||||
{
|
||||
TF_ZPG_t zpgc;
|
||||
int totDiff;
|
||||
int i;
|
||||
|
||||
zpgc.zerosLength = 0;
|
||||
zpgc.polesLength = 0;
|
||||
zpgc.gain = pkDifc->gain;
|
||||
zpgc.Ts = pkDifc->Ts;
|
||||
|
||||
// Total number of differentiators / integerators
|
||||
// positive diff, negative integ, 0 for no int/diff
|
||||
totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength;
|
||||
|
||||
while (zpgc.zerosLength < totDiff)
|
||||
{
|
||||
zpgc.zerosData[zpgc.zerosLength] = 0;
|
||||
zpgc.zerosLength++;
|
||||
}
|
||||
while (zpgc.polesLength < -totDiff)
|
||||
{
|
||||
zpgc.polesData[zpgc.polesLength] = 0;
|
||||
zpgc.polesLength++;
|
||||
}
|
||||
|
||||
// The next step is to calculate the poles
|
||||
// This has to be done for the highpass and lowpass filters
|
||||
// As we are using bilinear transformation there will be frequency
|
||||
// warping which has to be accounted for
|
||||
for (i = 0; i < pkDifc->lowpassLength; i++)
|
||||
{
|
||||
// pre-warping:
|
||||
double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0);
|
||||
// calculate pole:
|
||||
zpgc.polesData[zpgc.polesLength] = -freq;
|
||||
// adjust gain such that lp has gain = 1 for low frequencies
|
||||
zpgc.gain *= freq;
|
||||
zpgc.polesLength++;
|
||||
}
|
||||
for (i = 0; i < pkDifc->highpassLength; i++)
|
||||
{
|
||||
// pre-warping
|
||||
double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0);
|
||||
// calculate pole:
|
||||
zpgc.polesData[zpgc.polesLength] = -freq;
|
||||
// gain does not have to be adjusted
|
||||
zpgc.polesLength++;
|
||||
}
|
||||
|
||||
return btZpgcToZpgd(&zpgc, Ts, pZpgd);
|
||||
}
|
||||
|
||||
// bilinear transformation of poles and zeros
|
||||
int btZpgcToZpgd(const TF_ZPG_t *pkZpgc,
|
||||
double Ts,
|
||||
TF_ZPG_t *pZpgd)
|
||||
{
|
||||
int i;
|
||||
|
||||
// init digital gain
|
||||
pZpgd->gain = pkZpgc->gain;
|
||||
|
||||
// transform the poles
|
||||
pZpgd->polesLength = pkZpgc->polesLength;
|
||||
for (i = 0; i < pkZpgc->polesLength; i++)
|
||||
{
|
||||
pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]);
|
||||
pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]);
|
||||
}
|
||||
|
||||
// transform the zeros
|
||||
pZpgd->zerosLength = pkZpgc->zerosLength;
|
||||
for (i = 0; i < pkZpgc->zerosLength; i++)
|
||||
{
|
||||
pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]);
|
||||
pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]);
|
||||
}
|
||||
|
||||
// if we don't have the same number of poles as zeros we have to add
|
||||
// poles or zeros due to the bilinear transformation
|
||||
while (pZpgd->zerosLength < pZpgd->polesLength)
|
||||
{
|
||||
pZpgd->zerosData[pZpgd->zerosLength] = -1.0;
|
||||
pZpgd->zerosLength++;
|
||||
}
|
||||
while (pZpgd->zerosLength > pZpgd->polesLength)
|
||||
{
|
||||
pZpgd->polesData[pZpgd->polesLength] = -1.0;
|
||||
pZpgd->polesLength++;
|
||||
}
|
||||
|
||||
pZpgd->Ts = Ts;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
// calculate polynom from zero, pole, gain form
|
||||
int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly)
|
||||
{
|
||||
int i, j; // counter variable
|
||||
double tmp0, tmp1, gain;
|
||||
|
||||
// copy Ts
|
||||
pPoly->Ts = pkZpg->Ts;
|
||||
|
||||
// calculate denominator polynom
|
||||
pPoly->denLength = 1;
|
||||
pPoly->denData[0] = 1.0;
|
||||
for (i = 0; i < pkZpg->polesLength; i++)
|
||||
{
|
||||
// init temporary variable
|
||||
tmp0 = 0.0;
|
||||
// increase the polynom by 1
|
||||
pPoly->denData[pPoly->denLength] = 0;
|
||||
pPoly->denLength++;
|
||||
for (j = 0; j < pPoly->denLength; j++)
|
||||
{
|
||||
tmp1 = pPoly->denData[j];
|
||||
pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0;
|
||||
tmp0 = tmp1;
|
||||
}
|
||||
}
|
||||
|
||||
// calculate numerator polynom
|
||||
pPoly->numLength = 1;
|
||||
pPoly->numData[0] = pkZpg->gain;
|
||||
for (i = 0; i < pkZpg->zerosLength; i++)
|
||||
{
|
||||
tmp0 = 0.0;
|
||||
pPoly->numData[pPoly->numLength] = 0;
|
||||
pPoly->numLength++;
|
||||
for (j = 0; j < pPoly->numLength; j++)
|
||||
{
|
||||
tmp1 = pPoly->numData[j];
|
||||
pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0;
|
||||
tmp0 = tmp1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
#ifndef IIRFILTER__H__
|
||||
#define IIRFILTER__H__
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#define MAX_LENGTH 4
|
||||
|
||||
typedef struct FILTER_s
|
||||
{
|
||||
float denData[MAX_LENGTH];
|
||||
float numData[MAX_LENGTH];
|
||||
int denLength;
|
||||
int numLength;
|
||||
float Ts;
|
||||
float inpData[MAX_LENGTH];
|
||||
float outData[MAX_LENGTH];
|
||||
unsigned int inpCnt;
|
||||
} FIL_T;
|
||||
|
||||
typedef struct TF_ZPG_s
|
||||
{
|
||||
int zerosLength;
|
||||
double zerosData[MAX_LENGTH + 1];
|
||||
int polesLength;
|
||||
double polesData[MAX_LENGTH + 1];
|
||||
double gain;
|
||||
double Ts;
|
||||
} TF_ZPG_t;
|
||||
|
||||
typedef struct TF_POLY_s
|
||||
{
|
||||
int numLength;
|
||||
double numData[MAX_LENGTH];
|
||||
int denLength;
|
||||
double denData[MAX_LENGTH];
|
||||
double Ts;
|
||||
} TF_POLY_t;
|
||||
|
||||
typedef struct TF_DIF_s
|
||||
{
|
||||
int numInt;
|
||||
int numDiff;
|
||||
int lowpassLength;
|
||||
int highpassLength;
|
||||
double lowpassData[MAX_LENGTH];
|
||||
int highpassData[MAX_LENGTH];
|
||||
double gain;
|
||||
double Ts;
|
||||
} TF_DIF_t;
|
||||
|
||||
__EXPORT int testFunction(void);
|
||||
|
||||
__EXPORT float updateFilter(FIL_T *pFilt, float inp);
|
||||
|
||||
__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
|
|
@ -65,6 +65,7 @@
|
|||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include "iirFilter.h"
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
|
|
@ -124,7 +125,7 @@ static const int ERROR = -1;
|
|||
#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
|
||||
#define REG1_CONT_UPDATE_A (0<<3)
|
||||
#define REG1_BDU_UPDATE (1<<3)
|
||||
#define REG1_Z_ENABLE_A (1<<2)
|
||||
#define REG1_Y_ENABLE_A (1<<1)
|
||||
#define REG1_X_ENABLE_A (1<<0)
|
||||
|
|
@ -156,11 +157,11 @@ static const int ERROR = -1;
|
|||
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
||||
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
||||
|
||||
#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6))
|
||||
#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6))
|
||||
#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6))
|
||||
#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6))
|
||||
#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6))
|
||||
#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5))
|
||||
|
||||
#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
|
||||
|
||||
|
|
@ -218,6 +219,12 @@ private:
|
|||
float _accel_range_m_s2;
|
||||
orb_advert_t _accel_topic;
|
||||
|
||||
unsigned _current_samplerate;
|
||||
|
||||
// FIL_T _filter_x;
|
||||
// FIL_T _filter_y;
|
||||
// FIL_T _filter_z;
|
||||
|
||||
unsigned _num_mag_reports;
|
||||
volatile unsigned _next_mag_report;
|
||||
volatile unsigned _oldest_mag_report;
|
||||
|
|
@ -393,6 +400,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
|||
_accel_range_scale(0.0f),
|
||||
_accel_range_m_s2(0.0f),
|
||||
_accel_topic(-1),
|
||||
_current_samplerate(0),
|
||||
_num_mag_reports(0),
|
||||
_next_mag_report(0),
|
||||
_oldest_mag_report(0),
|
||||
|
|
@ -405,8 +413,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
|||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
_mag_range_scale = 12.0f/32767.0f;
|
||||
|
||||
// default scale factors
|
||||
_accel_scale.x_offset = 0;
|
||||
_accel_scale.x_scale = 1.0f;
|
||||
|
|
@ -476,7 +482,7 @@ LSM303D::init()
|
|||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
|
||||
|
||||
/* enable accel, XXX do this with an ioctl? */
|
||||
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
|
||||
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
|
||||
|
||||
/* enable mag, XXX do this with an ioctl? */
|
||||
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
|
||||
|
|
@ -488,6 +494,22 @@ LSM303D::init()
|
|||
set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
|
||||
set_samplerate(400); /* max sample rate */
|
||||
|
||||
/* initiate filter */
|
||||
// TF_DIF_t tf_filter;
|
||||
// tf_filter.numInt = 0;
|
||||
// tf_filter.numDiff = 0;
|
||||
// tf_filter.lowpassLength = 2;
|
||||
// tf_filter.highpassLength = 0;
|
||||
// tf_filter.lowpassData[0] = 10;
|
||||
// tf_filter.lowpassData[1] = 10;
|
||||
// //tf_filter.highpassData[0] = ;
|
||||
// tf_filter.gain = 1;
|
||||
// tf_filter.Ts = 1/_current_samplerate;
|
||||
//
|
||||
// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
|
||||
// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
|
||||
// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
|
||||
|
||||
mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
|
||||
mag_set_samplerate(100);
|
||||
|
||||
|
|
@ -705,6 +727,23 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
else
|
||||
return -EINVAL;
|
||||
|
||||
case ACCELIOCSSCALE:
|
||||
{
|
||||
/* copy scale, but only if off by a few percent */
|
||||
struct accel_scale *s = (struct accel_scale *) arg;
|
||||
float sum = s->x_scale + s->y_scale + s->z_scale;
|
||||
if (sum > 2.0f && sum < 4.0f) {
|
||||
memcpy(&_accel_scale, s, sizeof(_accel_scale));
|
||||
return OK;
|
||||
} else {
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
case ACCELIOCGSCALE:
|
||||
/* copy scale out */
|
||||
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
|
||||
return OK;
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
|
|
@ -856,37 +895,43 @@ LSM303D::set_range(unsigned max_g)
|
|||
{
|
||||
uint8_t setbits = 0;
|
||||
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
|
||||
float new_range = 0.0f;
|
||||
float new_range_g = 0.0f;
|
||||
float new_scale_g_digit = 0.0f;
|
||||
|
||||
if (max_g == 0)
|
||||
max_g = 16;
|
||||
|
||||
if (max_g <= 2) {
|
||||
new_range = 2.0f;
|
||||
new_range_g = 2.0f;
|
||||
setbits |= REG2_FULL_SCALE_2G_A;
|
||||
new_scale_g_digit = 0.061e-3f;
|
||||
|
||||
} else if (max_g <= 4) {
|
||||
new_range = 4.0f;
|
||||
new_range_g = 4.0f;
|
||||
setbits |= REG2_FULL_SCALE_4G_A;
|
||||
new_scale_g_digit = 0.122e-3f;
|
||||
|
||||
} else if (max_g <= 6) {
|
||||
new_range = 6.0f;
|
||||
new_range_g = 6.0f;
|
||||
setbits |= REG2_FULL_SCALE_6G_A;
|
||||
new_scale_g_digit = 0.183e-3f;
|
||||
|
||||
} else if (max_g <= 8) {
|
||||
new_range = 8.0f;
|
||||
new_range_g = 8.0f;
|
||||
setbits |= REG2_FULL_SCALE_8G_A;
|
||||
new_scale_g_digit = 0.244e-3f;
|
||||
|
||||
} else if (max_g <= 16) {
|
||||
new_range = 16.0f;
|
||||
new_range_g = 16.0f;
|
||||
setbits |= REG2_FULL_SCALE_16G_A;
|
||||
new_scale_g_digit = 0.732e-3f;
|
||||
|
||||
} else {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_accel_range_m_s2 = new_range * 9.80665f;
|
||||
_accel_range_scale = _accel_range_m_s2 / 32768.0f;
|
||||
_accel_range_m_s2 = new_range_g * 9.80665f;
|
||||
_accel_range_scale = new_scale_g_digit * 9.80665f;
|
||||
|
||||
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
||||
|
||||
|
|
@ -899,6 +944,7 @@ LSM303D::mag_set_range(unsigned max_ga)
|
|||
uint8_t setbits = 0;
|
||||
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
|
||||
float new_range = 0.0f;
|
||||
float new_scale_ga_digit = 0.0f;
|
||||
|
||||
if (max_ga == 0)
|
||||
max_ga = 12;
|
||||
|
|
@ -906,25 +952,29 @@ LSM303D::mag_set_range(unsigned max_ga)
|
|||
if (max_ga <= 2) {
|
||||
new_range = 2.0f;
|
||||
setbits |= REG6_FULL_SCALE_2GA_M;
|
||||
new_scale_ga_digit = 0.080e-3f;
|
||||
|
||||
} else if (max_ga <= 4) {
|
||||
new_range = 4.0f;
|
||||
setbits |= REG6_FULL_SCALE_4GA_M;
|
||||
new_scale_ga_digit = 0.160e-3f;
|
||||
|
||||
} else if (max_ga <= 8) {
|
||||
new_range = 8.0f;
|
||||
setbits |= REG6_FULL_SCALE_8GA_M;
|
||||
new_scale_ga_digit = 0.320e-3f;
|
||||
|
||||
} else if (max_ga <= 12) {
|
||||
new_range = 12.0f;
|
||||
setbits |= REG6_FULL_SCALE_12GA_M;
|
||||
new_scale_ga_digit = 0.479e-3f;
|
||||
|
||||
} else {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_mag_range_ga = new_range;
|
||||
_mag_range_scale = _mag_range_ga / 32768.0f;
|
||||
_mag_range_scale = new_scale_ga_digit;
|
||||
|
||||
modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
|
||||
|
||||
|
|
@ -991,18 +1041,23 @@ LSM303D::set_samplerate(unsigned frequency)
|
|||
|
||||
if (frequency <= 100) {
|
||||
setbits |= REG1_RATE_100HZ_A;
|
||||
_current_samplerate = 100;
|
||||
|
||||
} else if (frequency <= 200) {
|
||||
setbits |= REG1_RATE_200HZ_A;
|
||||
_current_samplerate = 200;
|
||||
|
||||
} else if (frequency <= 400) {
|
||||
setbits |= REG1_RATE_400HZ_A;
|
||||
_current_samplerate = 400;
|
||||
|
||||
} else if (frequency <= 800) {
|
||||
setbits |= REG1_RATE_800HZ_A;
|
||||
_current_samplerate = 800;
|
||||
|
||||
} else if (frequency <= 1600) {
|
||||
setbits |= REG1_RATE_1600HZ_A;
|
||||
_current_samplerate = 1600;
|
||||
|
||||
} else {
|
||||
return -EINVAL;
|
||||
|
|
@ -1127,9 +1182,18 @@ LSM303D::measure()
|
|||
accel_report->y_raw = raw_accel_report.y;
|
||||
accel_report->z_raw = raw_accel_report.z;
|
||||
|
||||
// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
//
|
||||
// accel_report->x = updateFilter(&_filter_x, x_in_new);
|
||||
// accel_report->y = updateFilter(&_filter_y, y_in_new);
|
||||
// accel_report->z = updateFilter(&_filter_z, z_in_new);
|
||||
|
||||
accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
|
||||
accel_report->scaling = _accel_range_scale;
|
||||
accel_report->range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
|
|
@ -1341,7 +1405,7 @@ void
|
|||
test()
|
||||
{
|
||||
int fd_accel = -1;
|
||||
struct accel_report a_report;
|
||||
struct accel_report accel_report;
|
||||
ssize_t sz;
|
||||
int filter_bandwidth;
|
||||
|
||||
|
|
@ -1352,20 +1416,20 @@ test()
|
|||
err(1, "%s open failed", ACCEL_DEVICE_PATH);
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd_accel, &a_report, sizeof(a_report));
|
||||
sz = read(fd_accel, &accel_report, sizeof(accel_report));
|
||||
|
||||
if (sz != sizeof(a_report))
|
||||
if (sz != sizeof(accel_report))
|
||||
err(1, "immediate read failed");
|
||||
|
||||
|
||||
warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
|
||||
warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
|
||||
warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
|
||||
warnx("accel x: \t%d\traw", (int)a_report.x_raw);
|
||||
warnx("accel y: \t%d\traw", (int)a_report.y_raw);
|
||||
warnx("accel z: \t%d\traw", (int)a_report.z_raw);
|
||||
warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
|
||||
warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
|
||||
warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
|
||||
warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
|
||||
warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
|
||||
warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
|
||||
|
||||
warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
|
||||
warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
|
||||
if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
|
||||
warnx("accel antialias filter bandwidth: fail");
|
||||
else
|
||||
|
|
|
|||
|
|
@ -3,4 +3,5 @@
|
|||
#
|
||||
|
||||
MODULE_COMMAND = lsm303d
|
||||
SRCS = lsm303d.cpp
|
||||
SRCS = lsm303d.cpp \
|
||||
iirFilter.c
|
||||
|
|
|
|||
|
|
@ -111,8 +111,9 @@ protected:
|
|||
|
||||
ms5611::prom_s _prom;
|
||||
|
||||
struct work_s _work;
|
||||
unsigned _measure_ticks;
|
||||
struct hrt_call _baro_call;
|
||||
|
||||
unsigned _call_baro_interval;
|
||||
|
||||
unsigned _num_reports;
|
||||
volatile unsigned _next_report;
|
||||
|
|
@ -143,12 +144,12 @@ protected:
|
|||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start_cycle();
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop_cycle();
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
|
|
@ -166,12 +167,11 @@ protected:
|
|||
void cycle();
|
||||
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
* Static trampoline; XXX is this needed?
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
void cycle_trampoline(void *arg);
|
||||
|
||||
/**
|
||||
* Issue a measurement command for the current state.
|
||||
|
|
@ -184,6 +184,7 @@ protected:
|
|||
* Collect the result of the most recent measurement.
|
||||
*/
|
||||
virtual int collect();
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
|||
CDev("MS5611", BARO_DEVICE_PATH),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
_measure_ticks(0),
|
||||
_call_baro_interval(0),
|
||||
_num_reports(0),
|
||||
_next_report(0),
|
||||
_oldest_report(0),
|
||||
|
|
@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
|||
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
|
||||
{
|
||||
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
|
||||
}
|
||||
|
||||
MS5611::~MS5611()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop_cycle();
|
||||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
|
|
@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
|||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
if (_call_baro_interval > 0) {
|
||||
|
||||
/*
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
|
|
@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
|||
|
||||
/* manual measurement - run one conversion */
|
||||
/* XXX really it'd be nice to lock against other readers here */
|
||||
do {
|
||||
_measure_phase = 0;
|
||||
_oldest_report = _next_report = 0;
|
||||
_measure_phase = 0;
|
||||
_oldest_report = _next_report = 0;
|
||||
measure();
|
||||
|
||||
/* do temperature first */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* now do a pressure measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
} while (0);
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop_cycle();
|
||||
_measure_ticks = 0;
|
||||
stop();
|
||||
_call_baro_interval = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling not supported */
|
||||
|
|
@ -362,14 +334,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
bool want_start = (_call_baro_interval == 0);
|
||||
|
||||
/* set interval to minimum legal value */
|
||||
_baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL;
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start_cycle();
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -377,21 +350,18 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
bool want_start = (_call_baro_interval == 0);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
|
||||
if (1000000/arg < MS5611_CONVERSION_INTERVAL)
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
/* update interval */
|
||||
_baro_call.period = _call_baro_interval = 1000000/arg;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start_cycle();
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -399,10 +369,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
if (_call_baro_interval == 0)
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
return (1000 / _call_baro_interval);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* add one to account for the sentinel in the ring */
|
||||
|
|
@ -419,11 +389,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
return -ENOMEM;
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop_cycle();
|
||||
stop();
|
||||
delete[] _reports;
|
||||
_num_reports = arg;
|
||||
_reports = buf;
|
||||
start_cycle();
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -457,30 +427,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
}
|
||||
|
||||
void
|
||||
MS5611::start_cycle()
|
||||
MS5611::start()
|
||||
{
|
||||
/* make sure we are stopped first */
|
||||
stop();
|
||||
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_measure_phase = 0;
|
||||
/* reset the report ring */
|
||||
_oldest_report = _next_report = 0;
|
||||
/* reset to first measure phase */
|
||||
_measure_phase = 0;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
|
||||
hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this);
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::stop_cycle()
|
||||
MS5611::stop()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
hrt_cancel(&_baro_call);
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::cycle_trampoline(void *arg)
|
||||
{
|
||||
MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
|
||||
MS5611 *dev = (MS5611 *)arg;
|
||||
|
||||
dev->cycle();
|
||||
cycle();
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -504,7 +476,7 @@ MS5611::cycle()
|
|||
//log("collection error %d", ret);
|
||||
}
|
||||
/* reset the collection state machine and try again */
|
||||
start_cycle();
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -517,14 +489,16 @@ MS5611::cycle()
|
|||
* doing pressure measurements at something close to the desired rate.
|
||||
*/
|
||||
if ((_measure_phase != 0) &&
|
||||
(_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
|
||||
(_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MS5611::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
// XXX maybe do something appropriate as well
|
||||
|
||||
// /* schedule a fresh cycle call when we are ready to measure again */
|
||||
// work_queue(HPWORK,
|
||||
// &_work,
|
||||
// (worker_t)&MS5611::cycle_trampoline,
|
||||
// this,
|
||||
// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
|
|
@ -535,19 +509,12 @@ MS5611::cycle()
|
|||
if (ret != OK) {
|
||||
//log("measure error %d", ret);
|
||||
/* reset the collection state machine and try again */
|
||||
start_cycle();
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MS5611::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(MS5611_CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -696,13 +663,14 @@ MS5611::collect()
|
|||
return OK;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
MS5611::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("poll interval: %u ticks\n", _call_baro_interval);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
printf("TEMP: %d\n", _TEMP);
|
||||
|
|
|
|||
|
|
@ -256,7 +256,7 @@ private:
|
|||
* @param offset Register offset to start writing at.
|
||||
* @param values Pointer to array of values to write.
|
||||
* @param num_values The number of values to write.
|
||||
* @return Zero if all values were successfully written.
|
||||
* @return OK if all values were successfully written.
|
||||
*/
|
||||
int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
|
||||
|
||||
|
|
@ -266,7 +266,7 @@ private:
|
|||
* @param page Register page to write to.
|
||||
* @param offset Register offset to write to.
|
||||
* @param value Value to write.
|
||||
* @return Zero if the value was written successfully.
|
||||
* @return OK if the value was written successfully.
|
||||
*/
|
||||
int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
|
||||
|
||||
|
|
@ -277,7 +277,7 @@ private:
|
|||
* @param offset Register offset to start reading from.
|
||||
* @param values Pointer to array where values should be stored.
|
||||
* @param num_values The number of values to read.
|
||||
* @return Zero if all values were successfully read.
|
||||
* @return OK if all values were successfully read.
|
||||
*/
|
||||
int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
|
||||
|
||||
|
|
@ -1149,9 +1149,11 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
|
|||
}
|
||||
|
||||
int ret = _interface->write((page << 8) | offset, (void *)values, num_values);
|
||||
if (ret != num_values)
|
||||
if (ret != num_values) {
|
||||
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
|
||||
return ret;
|
||||
return -1;
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -1169,10 +1171,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
|
|||
return -EINVAL;
|
||||
}
|
||||
|
||||
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
|
||||
if (ret != num_values)
|
||||
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
|
||||
if (ret != num_values) {
|
||||
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
|
||||
return ret;
|
||||
return -1;
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
uint32_t
|
||||
|
|
@ -1180,7 +1184,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
|
|||
{
|
||||
uint16_t value;
|
||||
|
||||
if (io_reg_get(page, offset, &value, 1))
|
||||
if (io_reg_get(page, offset, &value, 1) != OK)
|
||||
return _io_reg_get_error;
|
||||
|
||||
return value;
|
||||
|
|
@ -1193,7 +1197,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
|
|||
uint16_t value;
|
||||
|
||||
ret = io_reg_get(page, offset, &value, 1);
|
||||
if (ret)
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
value &= ~clearbits;
|
||||
value |= setbits;
|
||||
|
|
@ -1500,9 +1504,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
|||
|
||||
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
|
||||
|
||||
uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
|
||||
|
||||
*(uint32_t *)arg = value;
|
||||
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
|
||||
if (*(uint32_t *)arg == _io_reg_get_error)
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -1890,7 +1894,7 @@ px4io_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
PX4IO_Uploader *up;
|
||||
const char *fn[3];
|
||||
const char *fn[5];
|
||||
|
||||
/* work out what we're uploading... */
|
||||
if (argc > 2) {
|
||||
|
|
@ -1900,7 +1904,9 @@ px4io_main(int argc, char *argv[])
|
|||
} else {
|
||||
fn[0] = "/fs/microsd/px4io.bin";
|
||||
fn[1] = "/etc/px4io.bin";
|
||||
fn[2] = nullptr;
|
||||
fn[2] = "/fs/microsd/px4io2.bin";
|
||||
fn[3] = "/etc/px4io2.bin";
|
||||
fn[4] = nullptr;
|
||||
}
|
||||
|
||||
up = new PX4IO_Uploader;
|
||||
|
|
|
|||
Loading…
Reference in New Issue