forked from Archive/PX4-Autopilot
Merge branch 'navigator_new' into navigator_new_vector
This commit is contained in:
commit
63b7159cda
|
@ -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)
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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);
|
||||||
}
|
}
|
|
@ -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_ */
|
||||||
|
|
|
@ -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 )
|
|
|
@ -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
|
||||||
|
|
|
@ -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 )
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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 *************************************************************/
|
||||||
|
|
|
@ -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 *******************************************************/
|
||||||
|
|
||||||
|
|
|
@ -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,5 +3,4 @@
|
||||||
#
|
#
|
||||||
|
|
||||||
MODULE_COMMAND = fmu
|
MODULE_COMMAND = fmu
|
||||||
SRCS = fmu.cpp \
|
SRCS = fmu.cpp
|
||||||
../../modules/systemlib/pwm_limit/pwm_limit.c
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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) {
|
||||||
limit->time_armed = hrt_absolute_time();
|
|
||||||
|
/* set arming time for the first call */
|
||||||
|
if (limit->time_armed == 0) {
|
||||||
|
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,44 +101,47 @@ 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 = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
|
progress = diff * 10000 / RAMP_TIME_US;
|
||||||
for (unsigned i=0; i<num_channels; i++) {
|
|
||||||
|
|
||||||
uint16_t ramp_min_pwm;
|
for (unsigned i=0; i<num_channels; i++) {
|
||||||
|
|
||||||
/* if a disarmed pwm value was set, blend between disarmed and min */
|
uint16_t ramp_min_pwm;
|
||||||
if (disarmed_pwm[i] > 0) {
|
|
||||||
|
|
||||||
/* safeguard against overflows */
|
/* if a disarmed pwm value was set, blend between disarmed and min */
|
||||||
uint16_t disarmed = disarmed_pwm[i];
|
if (disarmed_pwm[i] > 0) {
|
||||||
if (disarmed > min_pwm[i])
|
|
||||||
disarmed = min_pwm[i];
|
|
||||||
|
|
||||||
uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
|
/* safeguard against overflows */
|
||||||
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
|
unsigned disarmed = disarmed_pwm[i];
|
||||||
} else {
|
if (disarmed > min_pwm[i]) {
|
||||||
|
disarmed = min_pwm[i];
|
||||||
|
}
|
||||||
|
|
||||||
/* no disarmed pwm value set, choose min pwm */
|
unsigned disarmed_min_diff = min_pwm[i] - disarmed;
|
||||||
ramp_min_pwm = min_pwm[i];
|
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
/* no disarmed pwm value set, choose min pwm */
|
||||||
|
ramp_min_pwm = min_pwm[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
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:
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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);
|
|
||||||
}
|
|
|
@ -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
|
|
|
@ -3,4 +3,4 @@
|
||||||
#
|
#
|
||||||
|
|
||||||
MODULE_COMMAND = mtd
|
MODULE_COMMAND = mtd
|
||||||
SRCS = mtd.c
|
SRCS = mtd.c 24xxxx_mtd.c
|
||||||
|
|
|
@ -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_readtest(partition_names_default, n_partitions_default);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
}
|
}
|
||||||
mtd_erase(argv + 2, argc - 2);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "expected a command, try 'start', 'erase' or 'test'");
|
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
|
||||||
|
|
|
@ -1,6 +0,0 @@
|
||||||
#
|
|
||||||
# RAMTRON file system driver
|
|
||||||
#
|
|
||||||
|
|
||||||
MODULE_COMMAND = ramtron
|
|
||||||
SRCS = ramtron.c
|
|
|
@ -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
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 */
|
||||||
|
|
|
@ -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,8 +366,12 @@ 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
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -32,8 +32,8 @@
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file test_ppm_loopback.c
|
* @file test_rc.c
|
||||||
* Tests the PWM outputs and PPM input
|
* Tests RC input.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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}
|
||||||
|
|
Loading…
Reference in New Issue