dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_new' into navigator_new_vector

This commit is contained in:
Anton Babushkin 2014-01-17 14:37:48 +01:00
parent 8ab3870a0d 73546b6645
commit 63b7159cda
31 changed files with 1028 additions and 907 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/tests-host/Makefile
View File

@ -10,11 +10,13 @@ LIBS=-lm
#_DEPS = test.h #_DEPS = test.h
#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o _OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
#$(DEPS) #$(DEPS)
$(ODIR)/%.o: %.cpp $(ODIR)/%.o: %.cpp
mkdir -p obj
$(CC) -c -o $@ $< $(CFLAGS) $(CC) -c -o $@ $< $(CFLAGS)
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
$(CC) -c -o $@ $< $(CFLAGS) $(CC) -c -o $@ $< $(CFLAGS)
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
$(CC) -c -o $@ $< $(CFLAGS)
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
$(CC) -c -o $@ $< $(CFLAGS)
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
$(CC) -c -o $@ $< $(CFLAGS) $(CC) -c -o $@ $< $(CFLAGS)

16
Tools/tests-host/hrt.cpp Normal file
View File

@ -0,0 +1,16 @@
#include <sys/time.h>
#include <inttypes.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
hrt_abstime hrt_absolute_time() {
struct timeval te;
gettimeofday(&te, NULL); // get current time
hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
return us;
}
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
// not thread safe
return hrt_absolute_time() - *then;
}

2
Tools/tests-host/mixer_test.cpp
View File

@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
"../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
test_mixer(3, args); test_mixer(3, args);
test_conv(1, args);
} }

133
Tools/tests-host/queue.h Normal file
View File

@ -0,0 +1,133 @@
/************************************************************************
* include/queue.h
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
************************************************************************/
#ifndef __INCLUDE_QUEUE_H
#define __INCLUDE_QUEUE_H
#ifndef FAR
#define FAR
#endif
/************************************************************************
* Included Files
************************************************************************/
#include <sys/types.h>
/************************************************************************
* Pre-processor Definitions
************************************************************************/
#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
#define sq_next(p) ((p)->flink)
#define dq_next(p) ((p)->flink)
#define dq_prev(p) ((p)->blink)
#define sq_empty(q) ((q)->head == NULL)
#define dq_empty(q) ((q)->head == NULL)
#define sq_peek(q) ((q)->head)
#define dq_peek(q) ((q)->head)
/************************************************************************
* Global Type Declarations
************************************************************************/
struct sq_entry_s
{
FAR struct sq_entry_s *flink;
};
typedef struct sq_entry_s sq_entry_t;
struct dq_entry_s
{
FAR struct dq_entry_s *flink;
FAR struct dq_entry_s *blink;
};
typedef struct dq_entry_s dq_entry_t;
struct sq_queue_s
{
FAR sq_entry_t *head;
FAR sq_entry_t *tail;
};
typedef struct sq_queue_s sq_queue_t;
struct dq_queue_s
{
FAR dq_entry_t *head;
FAR dq_entry_t *tail;
};
typedef struct dq_queue_s dq_queue_t;
/************************************************************************
* Global Function Prototypes
************************************************************************/
#ifdef __cplusplus
#define EXTERN extern "C"
extern "C" {
#else
#define EXTERN extern
#endif
EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
sq_queue_t *queue);
EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
dq_queue_t *queue);
EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
#undef EXTERN
#ifdef __cplusplus
}
#endif
#endif /* __INCLUDE_QUEUE_H_ */

148
makefiles/config_px4fmu-v1_backside.mk
View File

@ -1,148 +0,0 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/eeprom
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
#
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/att_pos_estimator_ekf
#
# Vehicle Control
#
MODULES += modules/fixedwing_backside
#
# Logging
#
MODULES += modules/sdlog2
#
# Unit tests
#
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
#
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
#MODULES += examples/px4_daemon_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
#MODULES += examples/fixedwing_control
# Hardware test
#MODULES += examples/hwtest
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main ) \
$(call _B, sysinfo, , 2048, sysinfo_main )

3
makefiles/config_px4fmu-v1_default.mk
View File

@ -42,8 +42,7 @@ MODULES += modules/sensors
# #
# System commands # System commands
# #
MODULES += systemcmds/eeprom MODULES += systemcmds/mtd
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c MODULES += systemcmds/i2c

48
makefiles/config_px4fmu-v1_test.mk
View File

@ -1,48 +0,0 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/px4io
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main )

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -46,7 +46,6 @@ MODULES += modules/sensors
# #
# System commands # System commands
# #
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo MODULES += systemcmds/boardinfo
MODULES += systemcmds/mixer MODULES += systemcmds/mixer

6
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -75,7 +75,7 @@ __BEGIN_DECLS
/* PX4FMU GPIOs ***********************************************************************************/ /* PX4FMU GPIOs ***********************************************************************************/
/* LEDs */ /* LEDs */
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
/* External interrupts */ /* External interrupts */
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
@ -87,7 +87,7 @@ __BEGIN_DECLS
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) #define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) #define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) #define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) #define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* SPI1 off */ /* SPI1 off */
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) #define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
@ -98,7 +98,7 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
/* SPI chip selects */ /* SPI chip selects */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)

6
src/drivers/boards/px4io-v1/board_config.h
View File

@ -58,11 +58,11 @@
/* PX4IO GPIOs **********************************************************************/ /* PX4IO GPIOs **********************************************************************/
/* LEDs */ /* LEDs */
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* Safety switch button *************************************************************/ /* Safety switch button *************************************************************/

6
src/drivers/boards/px4io-v2/board_config.h
View File

@ -74,9 +74,9 @@
/* LEDS **********************************************************************/ /* LEDS **********************************************************************/
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
/* Safety switch button *******************************************************/ /* Safety switch button *******************************************************/

11
src/drivers/px4fmu/fmu.cpp
View File

@ -1108,10 +1108,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF);
@ -1124,10 +1126,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */ /* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
@ -1160,6 +1164,13 @@ PX4FMU::sensor_reset(int ms)
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
// // XXX bring up the EXTI pins again
// stm32_configgpio(GPIO_GYRO_DRDY);
// stm32_configgpio(GPIO_MAG_DRDY);
// stm32_configgpio(GPIO_ACCEL_DRDY);
// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif #endif
#endif #endif
} }

3
src/drivers/px4fmu/module.mk
View File

@ -3,5 +3,4 @@
# #
MODULE_COMMAND = fmu MODULE_COMMAND = fmu
SRCS = fmu.cpp \ SRCS = fmu.cpp
../../modules/systemlib/pwm_limit/pwm_limit.c

3
src/modules/systemlib/module.mk
View File

@ -51,5 +51,6 @@ SRCS = err.c \
mavlink_log.c \ mavlink_log.c \
rc_check.c \ rc_check.c \
otp.c \ otp.c \
board_serial.c board_serial.c \
pwm_limit/pwm_limit.c

78
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch> * Author: Julian Oes <joes@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
@ -44,38 +44,53 @@
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <stdio.h>
void pwm_limit_init(pwm_limit_t *limit) void pwm_limit_init(pwm_limit_t *limit)
{ {
limit->state = LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_INIT;
limit->time_armed = 0; limit->time_armed = 0;
return; return;
} }
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{ {
/* first evaluate state changes */ /* first evaluate state changes */
switch (limit->state) { switch (limit->state) {
case LIMIT_STATE_OFF: case PWM_LIMIT_STATE_INIT:
if (armed)
limit->state = LIMIT_STATE_RAMP; if (armed) {
/* set arming time for the first call */
if (limit->time_armed == 0) {
limit->time_armed = hrt_absolute_time(); limit->time_armed = hrt_absolute_time();
}
if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF;
}
}
break; break;
case LIMIT_STATE_INIT: case PWM_LIMIT_STATE_OFF:
if (!armed) if (armed) {
limit->state = LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_RAMP;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
limit->state = LIMIT_STATE_RAMP; /* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time();
}
break; break;
case LIMIT_STATE_RAMP: case PWM_LIMIT_STATE_RAMP:
if (!armed) if (!armed) {
limit->state = LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = LIMIT_STATE_ON; limit->state = PWM_LIMIT_STATE_ON;
}
break; break;
case LIMIT_STATE_ON: case PWM_LIMIT_STATE_ON:
if (!armed) if (!armed) {
limit->state = LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_OFF;
}
break; break;
default: default:
break; break;
@ -86,16 +101,18 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
/* then set effective_pwm based on state */ /* then set effective_pwm based on state */
switch (limit->state) { switch (limit->state) {
case LIMIT_STATE_OFF: case PWM_LIMIT_STATE_OFF:
case LIMIT_STATE_INIT: case PWM_LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i]; effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
} }
break; break;
case LIMIT_STATE_RAMP: case PWM_LIMIT_STATE_RAMP:
{
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
progress = diff * 10000 / RAMP_TIME_US;
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm; uint16_t ramp_min_pwm;
@ -104,12 +121,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
if (disarmed_pwm[i] > 0) { if (disarmed_pwm[i] > 0) {
/* safeguard against overflows */ /* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i]; unsigned disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i]) if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i]; disarmed = min_pwm[i];
}
uint16_t disarmed_min_diff = min_pwm[i] - disarmed; unsigned disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else { } else {
/* no disarmed pwm value set, choose min pwm */ /* no disarmed pwm value set, choose min pwm */
@ -117,13 +136,12 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
} }
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
output[i] = (float)progress/10000.0f * output[i]; }
} }
break; break;
case LIMIT_STATE_ON: case PWM_LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* effective_output stays the same */
} }
break; break;
default: default:

20
src/modules/systemlib/pwm_limit/pwm_limit.h
View File

@ -46,6 +46,8 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
__BEGIN_DECLS
/* /*
* time for the ESCs to initialize * time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot) * (this is not actually needed if PWM is sent right after boot)
@ -56,21 +58,21 @@
*/ */
#define RAMP_TIME_US 2500000 #define RAMP_TIME_US 2500000
enum pwm_limit_state {
PWM_LIMIT_STATE_OFF = 0,
PWM_LIMIT_STATE_INIT,
PWM_LIMIT_STATE_RAMP,
PWM_LIMIT_STATE_ON
};
typedef struct { typedef struct {
enum { enum pwm_limit_state state;
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
uint64_t time_armed; uint64_t time_armed;
} pwm_limit_t; } pwm_limit_t;
__BEGIN_DECLS
__EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS __END_DECLS

265
src/systemcmds/eeprom/eeprom.c
View File

@ -1,265 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 eeprom.c
*
* EEPROM service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <board_config.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
#ifndef PX4_I2C_BUS_ONBOARD
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
#endif
__EXPORT int eeprom_main(int argc, char *argv[]);
static void eeprom_attach(void);
static void eeprom_start(void);
static void eeprom_erase(void);
static void eeprom_ioctl(unsigned operation);
static void eeprom_save(const char *name);
static void eeprom_load(const char *name);
static void eeprom_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *eeprom_mtd;
int eeprom_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
eeprom_start();
if (!strcmp(argv[1], "save_param"))
eeprom_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
eeprom_load(argv[2]);
if (!strcmp(argv[1], "erase"))
eeprom_erase();
if (!strcmp(argv[1], "test"))
eeprom_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
eeprom_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
eeprom_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
}
static void
eeprom_attach(void)
{
/* find the right I2C */
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
/* this resets the I2C bus, set correct bus speed again */
I2C_SETFREQUENCY(i2c, 400000);
if (i2c == NULL)
errx(1, "failed to locate I2C bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
eeprom_mtd = at24c_initialize(i2c);
if (eeprom_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: EEPROM needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (eeprom_mtd == NULL)
errx(1, "failed to initialize EEPROM driver");
attached = true;
}
static void
eeprom_start(void)
{
int ret;
if (started)
errx(1, "EEPROM already mounted");
if (!attached)
eeprom_attach();
/* start NXFFS */
ret = nxffs_initialize(eeprom_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
/* mount the EEPROM */
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
started = true;
warnx("mounted EEPROM at /eeprom");
exit(0);
}
extern int at24c_nuke(void);
static void
eeprom_erase(void)
{
if (!attached)
eeprom_attach();
if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
eeprom_ioctl(unsigned operation)
{
int fd;
fd = open("/eeprom/.", 0);
if (fd < 0)
err(1, "open /eeprom");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
eeprom_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
eeprom_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
extern void at24c_test(void);
static void
eeprom_test(void)
{
at24c_test();
exit(0);
}

39
src/systemcmds/eeprom/module.mk
View File

@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 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.
#
############################################################################
#
# EEPROM file system driver
#
MODULE_COMMAND = eeprom
SRCS = 24xxxx_mtd.c eeprom.c

0
src/systemcmds/eeprom/24xxxx_mtd.c → src/systemcmds/mtd/24xxxx_mtd.c
View File

2
src/systemcmds/mtd/module.mk
View File

@ -3,4 +3,4 @@
# #
MODULE_COMMAND = mtd MODULE_COMMAND = mtd
SRCS = mtd.c SRCS = mtd.c 24xxxx_mtd.c

304
src/systemcmds/mtd/mtd.c
View File

@ -62,26 +62,43 @@
#include "systemlib/param/param.h" #include "systemlib/param/param.h"
#include "systemlib/err.h" #include "systemlib/err.h"
#include <board_config.h>
__EXPORT int mtd_main(int argc, char *argv[]); __EXPORT int mtd_main(int argc, char *argv[]);
#ifndef CONFIG_MTD_RAMTRON #ifndef CONFIG_MTD
/* create a fake command with decent warnx to not confuse users */ /* create a fake command with decent warnx to not confuse users */
int mtd_main(int argc, char *argv[]) int mtd_main(int argc, char *argv[])
{ {
errx(1, "RAMTRON not enabled, skipping."); errx(1, "MTD not enabled, skipping.");
} }
#else #else
static void mtd_attach(void); #ifdef CONFIG_MTD_RAMTRON
static void ramtron_attach(void);
#else
#ifndef PX4_I2C_BUS_ONBOARD
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
#endif
static void at24xxx_attach(void);
#endif
static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_start(char *partition_names[], unsigned n_partitions);
static void mtd_test(void); static void mtd_test(void);
static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_erase(char *partition_names[], unsigned n_partitions);
static void mtd_readtest(char *partition_names[], unsigned n_partitions);
static void mtd_rwtest(char *partition_names[], unsigned n_partitions);
static void mtd_print_info();
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
static bool attached = false; static bool attached = false;
static bool started = false; static bool started = false;
static struct mtd_dev_s *mtd_dev; static struct mtd_dev_s *mtd_dev;
static unsigned n_partitions_current = 0;
/* note, these will be equally sized */ /* note, these will be equally sized */
static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
@ -93,9 +110,8 @@ int mtd_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
/* start mapping according to user request */ /* start mapping according to user request */
if (argc > 3) { if (argc >= 3) {
mtd_start(argv + 2, argc - 2); mtd_start(argv + 2, argc - 2);
} else { } else {
mtd_start(partition_names_default, n_partitions_default); mtd_start(partition_names_default, n_partitions_default);
} }
@ -104,28 +120,49 @@ int mtd_main(int argc, char *argv[])
if (!strcmp(argv[1], "test")) if (!strcmp(argv[1], "test"))
mtd_test(); mtd_test();
if (!strcmp(argv[1], "erase")) { if (!strcmp(argv[1], "readtest")) {
if (argc < 3) { if (argc >= 3) {
errx(1, "usage: mtd erase <PARTITION_PATH..>"); mtd_readtest(argv + 2, argc - 2);
} } else {
mtd_erase(argv + 2, argc - 2); mtd_readtest(partition_names_default, n_partitions_default);
} }
} }
errx(1, "expected a command, try 'start', 'erase' or 'test'"); if (!strcmp(argv[1], "rwtest")) {
if (argc >= 3) {
mtd_rwtest(argv + 2, argc - 2);
} else {
mtd_rwtest(partition_names_default, n_partitions_default);
}
}
if (!strcmp(argv[1], "status"))
mtd_status();
if (!strcmp(argv[1], "erase")) {
if (argc >= 3) {
mtd_erase(argv + 2, argc - 2);
} else {
mtd_erase(partition_names_default, n_partitions_default);
}
}
}
errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
} }
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
off_t firstblock, off_t nblocks); off_t firstblock, off_t nblocks);
#ifdef CONFIG_MTD_RAMTRON
static void static void
mtd_attach(void) ramtron_attach(void)
{ {
/* find the right spi */ /* find the right spi */
struct spi_dev_s *spi = up_spiinitialize(2); struct spi_dev_s *spi = up_spiinitialize(2);
/* this resets the spi bus, set correct bus speed again */ /* this resets the spi bus, set correct bus speed again */
SPI_SETFREQUENCY(spi, 40 * 1000 * 1000); SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
SPI_SETBITS(spi, 8); SPI_SETBITS(spi, 8);
SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false); SPI_SELECT(spi, SPIDEV_FLASH, false);
@ -133,7 +170,7 @@ mtd_attach(void)
if (spi == NULL) if (spi == NULL)
errx(1, "failed to locate spi bus"); errx(1, "failed to locate spi bus");
/* start the MTD driver, attempt 5 times */ /* start the RAMTRON driver, attempt 5 times */
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
mtd_dev = ramtron_initialize(spi); mtd_dev = ramtron_initialize(spi);
@ -151,8 +188,44 @@ mtd_attach(void)
if (mtd_dev == NULL) if (mtd_dev == NULL)
errx(1, "failed to initialize mtd driver"); errx(1, "failed to initialize mtd driver");
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
if (ret != OK)
warnx(1, "failed to set bus speed");
attached = true; attached = true;
} }
#else
static void
at24xxx_attach(void)
{
/* find the right I2C */
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
/* this resets the I2C bus, set correct bus speed again */
I2C_SETFREQUENCY(i2c, 400000);
if (i2c == NULL)
errx(1, "failed to locate I2C bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
mtd_dev = at24c_initialize(i2c);
if (mtd_dev) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: EEPROM needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (mtd_dev == NULL)
errx(1, "failed to initialize EEPROM driver");
attached = true;
}
#endif
static void static void
mtd_start(char *partition_names[], unsigned n_partitions) mtd_start(char *partition_names[], unsigned n_partitions)
@ -162,42 +235,25 @@ mtd_start(char *partition_names[], unsigned n_partitions)
if (started) if (started)
errx(1, "mtd already mounted"); errx(1, "mtd already mounted");
if (!attached) if (!attached) {
mtd_attach(); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
at24xxx_attach();
#else
ramtron_attach();
#endif
}
if (!mtd_dev) { if (!mtd_dev) {
warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
exit(1); exit(1);
} }
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize;
/* Get the geometry of the FLASH device */ ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
if (ret)
FAR struct mtd_geometry_s geo;
ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
if (ret < 0) {
warnx("ERROR: mtd->ioctl failed: %d", ret);
exit(3); exit(3);
}
warnx("Flash Geometry:");
warnx(" blocksize: %lu", (unsigned long)geo.blocksize);
warnx(" erasesize: %lu", (unsigned long)geo.erasesize);
warnx(" neraseblocks: %lu", (unsigned long)geo.neraseblocks);
/* Determine the size of each partition. Make each partition an even
* multiple of the erase block size (perhaps not using some space at the
* end of the FLASH).
*/
unsigned blkpererase = geo.erasesize / geo.blocksize;
unsigned nblocks = (geo.neraseblocks / n_partitions) * blkpererase;
unsigned partsize = nblocks * geo.blocksize;
warnx(" No. partitions: %u", n_partitions);
warnx(" Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize);
/* Now create MTD FLASH partitions */ /* Now create MTD FLASH partitions */
@ -244,18 +300,98 @@ mtd_start(char *partition_names[], unsigned n_partitions)
} }
} }
n_partitions_current = n_partitions;
started = true; started = true;
exit(0); exit(0);
} }
static void int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
{
/* Get the geometry of the FLASH device */
FAR struct mtd_geometry_s geo;
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
if (ret < 0) {
warnx("ERROR: mtd->ioctl failed: %d", ret);
return ret;
}
*blocksize = geo.blocksize;
*erasesize = geo.blocksize;
*neraseblocks = geo.neraseblocks;
/* Determine the size of each partition. Make each partition an even
* multiple of the erase block size (perhaps not using some space at the
* end of the FLASH).
*/
*blkpererase = geo.erasesize / geo.blocksize;
*nblocks = (geo.neraseblocks / n_partitions) * *blkpererase;
*partsize = *nblocks * geo.blocksize;
return ret;
}
/*
get partition size in bytes
*/
static ssize_t mtd_get_partition_size(void)
{
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize = 0;
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
if (ret != OK) {
errx(1, "Failed to get geometry");
}
return partsize;
}
void mtd_print_info()
{
if (!attached)
exit(1);
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize;
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
if (ret)
exit(3);
warnx("Flash Geometry:");
printf(" blocksize: %lu\n", blocksize);
printf(" erasesize: %lu\n", erasesize);
printf(" neraseblocks: %lu\n", neraseblocks);
printf(" No. partitions: %u\n", n_partitions_current);
printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize);
printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024);
}
void
mtd_test(void) mtd_test(void)
{ {
warnx("This test routine does not test anything yet!"); warnx("This test routine does not test anything yet!");
exit(1); exit(1);
} }
static void void
mtd_status(void)
{
if (!attached)
errx(1, "MTD driver not started");
mtd_print_info();
exit(0);
}
void
mtd_erase(char *partition_names[], unsigned n_partitions) mtd_erase(char *partition_names[], unsigned n_partitions)
{ {
uint8_t v[64]; uint8_t v[64];
@ -267,7 +403,7 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
if (fd == -1) { if (fd == -1) {
errx(1, "Failed to open partition"); errx(1, "Failed to open partition");
} }
while (write(fd, &v, sizeof(v)) == sizeof(v)) { while (write(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v); count += sizeof(v);
} }
printf("Erased %lu bytes\n", (unsigned long)count); printf("Erased %lu bytes\n", (unsigned long)count);
@ -276,4 +412,82 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
exit(0); exit(0);
} }
/*
readtest is useful during startup to validate the device is
responding on the bus. It relies on the driver returning an error on
bad reads (the ramtron driver does return an error)
*/
void
mtd_readtest(char *partition_names[], unsigned n_partitions)
{
ssize_t expected_size = mtd_get_partition_size();
uint8_t v[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (read(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
}
if (count != expected_size) {
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
}
close(fd);
}
printf("readtest OK\n");
exit(0);
}
/*
rwtest is useful during startup to validate the device is
responding on the bus for both reads and writes. It reads data in
blocks and writes the data back, then reads it again, failing if the
data isn't the same
*/
void
mtd_rwtest(char *partition_names[], unsigned n_partitions)
{
ssize_t expected_size = mtd_get_partition_size();
uint8_t v[128], v2[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
off_t offset = 0;
printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDWR);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (read(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (write(fd, v, sizeof(v)) != sizeof(v)) {
errx(1, "write failed");
}
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
errx(1, "read failed");
}
if (memcmp(v, v2, sizeof(v2)) != 0) {
errx(1, "memcmp failed");
}
offset += sizeof(v);
}
if (count != expected_size) {
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
}
close(fd);
}
printf("rwtest OK\n");
exit(0);
}
#endif #endif

6
src/systemcmds/ramtron/module.mk
View File

@ -1,6 +0,0 @@
#
# RAMTRON file system driver
#
MODULE_COMMAND = ramtron
SRCS = ramtron.c

279
src/systemcmds/ramtron/ramtron.c
View File

@ -1,279 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 ramtron.c
*
* ramtron service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/spi.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int ramtron_main(int argc, char *argv[]);
#ifndef CONFIG_MTD_RAMTRON
/* create a fake command with decent message to not confuse users */
int ramtron_main(int argc, char *argv[])
{
errx(1, "RAMTRON not enabled, skipping.");
}
#else
static void ramtron_attach(void);
static void ramtron_start(void);
static void ramtron_erase(void);
static void ramtron_ioctl(unsigned operation);
static void ramtron_save(const char *name);
static void ramtron_load(const char *name);
static void ramtron_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *ramtron_mtd;
int ramtron_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
ramtron_start();
if (!strcmp(argv[1], "save_param"))
ramtron_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
ramtron_load(argv[2]);
if (!strcmp(argv[1], "erase"))
ramtron_erase();
if (!strcmp(argv[1], "test"))
ramtron_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
ramtron_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
ramtron_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
}
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
static void
ramtron_attach(void)
{
/* find the right spi */
struct spi_dev_s *spi = up_spiinitialize(2);
/* this resets the spi bus, set correct bus speed again */
// xxx set in ramtron driver, leave this out
// SPI_SETFREQUENCY(spi, 4000000);
SPI_SETFREQUENCY(spi, 375000000);
SPI_SETBITS(spi, 8);
SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false);
if (spi == NULL)
errx(1, "failed to locate spi bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
ramtron_mtd = ramtron_initialize(spi);
if (ramtron_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: ramtron needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (ramtron_mtd == NULL)
errx(1, "failed to initialize ramtron driver");
attached = true;
}
static void
ramtron_start(void)
{
int ret;
if (started)
errx(1, "ramtron already mounted");
if (!attached)
ramtron_attach();
/* start NXFFS */
ret = nxffs_initialize(ramtron_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
/* mount the ramtron */
ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /ramtron - erase ramtron to reformat");
started = true;
warnx("mounted ramtron at /ramtron");
exit(0);
}
//extern int at24c_nuke(void);
static void
ramtron_erase(void)
{
if (!attached)
ramtron_attach();
// if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
ramtron_ioctl(unsigned operation)
{
int fd;
fd = open("/ramtron/.", 0);
if (fd < 0)
err(1, "open /ramtron");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
ramtron_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
ramtron_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
//extern void at24c_test(void);
static void
ramtron_test(void)
{
// at24c_test();
exit(0);
}
#endif

4
src/systemcmds/tests/module.mk
View File

@ -30,4 +30,6 @@ SRCS = test_adc.c \
test_param.c \ test_param.c \
test_ppm_loopback.c \ test_ppm_loopback.c \
test_rc.c \ test_rc.c \
test_mount.c test_conv.cpp \
test_mount.c \
test_mtd.c

76
src/systemcmds/tests/test_conv.cpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2012, 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 test_conv.cpp
* Tests conversions used across the system.
*
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests.h"
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <unit_test/unit_test.h>
#include <px4iofirmware/protocol.h>
int test_conv(int argc, char *argv[])
{
warnx("Testing system conversions");
for (int i = -10000; i <= 10000; i+=1) {
float f = i/10000.0f;
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
if (fabsf(f - fres) > 0.0001f) {
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
return 1;
}
}
warnx("All conversions clean");
return 0;
}

2
src/systemcmds/tests/test_file.c
View File

@ -54,7 +54,7 @@
#include "tests.h" #include "tests.h"
int check_user_abort(int fd); static int check_user_abort(int fd);
int check_user_abort(int fd) { int check_user_abort(int fd) {
/* check if user wants to abort */ /* check if user wants to abort */

197
src/systemcmds/tests/test_mixer.cpp
View File

@ -48,9 +48,13 @@
#include <errno.h> #include <errno.h>
#include <string.h> #include <string.h>
#include <time.h> #include <time.h>
#include <math.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/mixer/mixer.h> #include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include "tests.h" #include "tests.h"
@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index, uint8_t control_index,
float &control); float &control);
const unsigned output_max = 8;
static float actuator_controls[output_max];
static bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
int test_mixer(int argc, char *argv[]) int test_mixer(int argc, char *argv[])
{ {
warnx("testing mixer"); warnx("testing mixer");
@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
if (mixer_group.count() != 8) if (mixer_group.count() != 8)
return 1; return 1;
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
should_arm = true;
/* run through arming phase */
for (int i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
warnx("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
unsigned sleepcount = 0;
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("disarmed servo value mismatch");
return 1;
}
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
warnx("ramp servo value mismatch");
return 1;
}
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
warnx("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (int i = 0; i < output_max; i++) {
actuator_controls[i] = j/10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
warnx("mixed %d outputs (max %d)", mixed, output_max);
for (int i = 0; i < mixed; i++)
{
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
}
}
warnx("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("disarmed servo value mismatch");
return 1;
}
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
warnx("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
{
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
warnx("ramp servo value mismatch");
return 1;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
/* load multirotor at once test */ /* load multirotor at once test */
mixer_group.reset(); mixer_group.reset();
@ -180,10 +366,14 @@ int test_mixer(int argc, char *argv[])
unsigned mc_loaded = loaded; unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded); mixer_group.load_from_buf(&buf[0], mc_loaded);
warnx("complete buffer load: loaded %u mixers", mixer_group.count()); warnx("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) if (mixer_group.count() != 5) {
warnx("FAIL: Quad W mixer load failed");
return 1; return 1;
} }
warnx("SUCCESS: No errors in mixer test");
}
static int static int
mixer_callback(uintptr_t handle, mixer_callback(uintptr_t handle,
uint8_t control_group, uint8_t control_group,
@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
if (control_group != 0) if (control_group != 0)
return -1; return -1;
control = 0.0f; if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
return -1;
control = actuator_controls[control_index];
return 0; return 0;
} }

229
src/systemcmds/tests/test_mtd.c Normal file
View File

@ -0,0 +1,229 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 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 test_mtd.c
*
* Param storage / file test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <poll.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
#define PARAM_FILE_NAME "/fs/mtd_params"
static int check_user_abort(int fd);
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
}
}
return 1;
}
void print_fail()
{
printf("<[T]: MTD: FAIL>\n");
}
void print_success()
{
printf("<[T]: MTD: OK>\n");
}
int
test_mtd(int argc, char *argv[])
{
unsigned iterations= 0;
/* check if microSD card is mounted */
struct stat buffer;
if (stat(PARAM_FILE_NAME, &buffer)) {
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
print_fail();
return 1;
}
// XXX get real storage space here
unsigned file_size = 4096;
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {256, 512, 4096};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open(PARAM_FILE_NAME, O_RDONLY);
int rret = read(fd, read_buf, chunk_sizes[c]);
close(fd);
fd = open(PARAM_FILE_NAME, O_WRONLY);
printf("printing 2 percent of the first chunk:\n");
for (int i = 0; i < sizeof(read_buf) / 50; i++) {
printf("%02X", read_buf[i]);
}
printf("\n");
iterations = file_size / chunk_sizes[c];
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
print_fail();
return 1;
}
fsync(fd);
if (!check_user_abort(fd))
return OK;
}
end = hrt_absolute_time();
close(fd);
fd = open(PARAM_FILE_NAME, O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
warnx("READ ERROR!");
print_fail();
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d", j);
print_fail();
compare_ok = false;
break;
}
}
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
print_fail();
return 1;
}
if (!check_user_abort(fd))
return OK;
}
close(fd);
}
/* fill the file with 0xFF to make it look new again */
char ffbuf[64];
memset(ffbuf, 0xFF, sizeof(ffbuf));
int fd = open(PARAM_FILE_NAME, O_WRONLY);
for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
int ret = write(fd, ffbuf, sizeof(ffbuf));
if (ret != sizeof(ffbuf)) {
warnx("ERROR! Could not fill file with 0xFF");
close(fd);
print_fail();
return 1;
}
}
(void)close(fd);
print_success();
return 0;
}

4
src/systemcmds/tests/test_rc.c
View File

@ -32,8 +32,8 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file test_ppm_loopback.c * @file test_rc.c
* Tests the PWM outputs and PPM input * Tests RC input.
* *
*/ */

2
src/systemcmds/tests/tests.h
View File

@ -109,7 +109,9 @@ extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]); extern int test_file(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]);
extern int test_mtd(int argc, char *argv[]);
extern int test_mathlib(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]);
__END_DECLS __END_DECLS

2
src/systemcmds/tests/tests_main.c
View File

@ -106,7 +106,9 @@ const struct {
{"file", test_file, 0}, {"file", test_file, 0},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mtd", test_mtd, 0},
{"mathlib", test_mathlib, 0}, {"mathlib", test_mathlib, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0} {NULL, NULL, 0}