fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into navigator_new

Browse Source 
Conflicts:
	makefiles/config_px4fmu-v1_backside.mk
	src/modules/commander/commander.cpp
	src/modules/sdlog2/sdlog2.c
This commit is contained in:
Thomas Gubler
2014-01-17 09:50:22 +01:00
parent 14c0fae175 e691bab71a
commit 73546b6645
55 changed files with 1965 additions and 1133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+10 -9
View File
@@ -75,14 +75,15 @@ then
#
# Load microSD params
#
#if ramtron start
#then
# param select /ramtron/params
# if [ -f /ramtron/params ]
# then
# param load /ramtron/params
# fi
#else
if mtd start
then
param select /fs/mtd_params
if param load /fs/mtd_params
then
else
echo "FAILED LOADING PARAMS"
fi
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
@@ -93,7 +94,7 @@ then
echo "Parameter file corrupt - ignoring"
fi
fi
#fi
fi
#
# Start system state indicator
Tools/px4params/dokuwikiout.py
+20 -10
View File
@@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output):
result = ""
for group in groups:
result += "==== %s ====\n\n" % group.GetName()
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
for param in group.GetParams():
code = param.GetFieldValue("code")
name = param.GetFieldValue("short_desc")
if code != name:
name = "%s (%s)" % (name, code)
result += "=== %s ===\n\n" % name
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
result += "%s\n\n" % long_desc
name = name.replace("\n", "")
result += "| %s | %s " % (code, name)
min_val = param.GetFieldValue("min")
if min_val is not None:
result += "* Minimal value: %s\n" % min_val
result += "| %s " % min_val
else:
result += "|"
max_val = param.GetFieldValue("max")
if max_val is not None:
result += "* Maximal value: %s\n" % max_val
result += "| %s " % max_val
else:
result += "|"
def_val = param.GetFieldValue("default")
if def_val is not None:
result += "* Default value: %s\n" % def_val
result += "\n"
result += "| %s " % def_val
else:
result += "|"
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
long_desc = long_desc.replace("\n", "")
result += "| %s " % long_desc
else:
result += "|"
result += "|\n"
result += "\n"
return result
Tools/px4params/dokuwikiout_listings.py
+27
View File
@@ -0,0 +1,27 @@
import output
class DokuWikiOutput(output.Output):
def Generate(self, groups):
result = ""
for group in groups:
result += "==== %s ====\n\n" % group.GetName()
for param in group.GetParams():
code = param.GetFieldValue("code")
name = param.GetFieldValue("short_desc")
if code != name:
name = "%s (%s)" % (name, code)
result += "=== %s ===\n\n" % name
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
result += "%s\n\n" % long_desc
min_val = param.GetFieldValue("min")
if min_val is not None:
result += "* Minimal value: %s\n" % min_val
max_val = param.GetFieldValue("max")
if max_val is not None:
result += "* Maximal value: %s\n" % max_val
def_val = param.GetFieldValue("default")
if def_val is not None:
result += "* Default value: %s\n" % def_val
result += "\n"
return result
Tools/tests-host/Makefile
+9 -1
View File
@@ -10,11 +10,13 @@ LIBS=-lm
#_DEPS = test.h
#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))
#$(DEPS)
$(ODIR)/%.o: %.cpp
mkdir -p obj
$(CC) -c -o $@ $< $(CFLAGS)
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
@@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
$(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
$(CC) -c -o $@ $< $(CFLAGS)
Tools/tests-host/hrt.cpp
+16
View File
@@ -0,0 +1,16 @@
#include <sys/time.h>
#include <inttypes.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
hrt_abstime hrt_absolute_time() {
struct timeval te;
gettimeofday(&te, NULL); // get current time
hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
return us;
}
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
// not thread safe
return hrt_absolute_time() - *then;
}
Tools/tests-host/mixer_test.cpp
+2
View File
@@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
"../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
test_mixer(3, args);
test_conv(1, args);
}
Tools/tests-host/queue.h
+133
View File
@@ -0,0 +1,133 @@
/************************************************************************
* include/queue.h
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
************************************************************************/
#ifndef __INCLUDE_QUEUE_H
#define __INCLUDE_QUEUE_H
#ifndef FAR
#define FAR
#endif
/************************************************************************
* Included Files
************************************************************************/
#include <sys/types.h>
/************************************************************************
* Pre-processor Definitions
************************************************************************/
#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
#define sq_next(p) ((p)->flink)
#define dq_next(p) ((p)->flink)
#define dq_prev(p) ((p)->blink)
#define sq_empty(q) ((q)->head == NULL)
#define dq_empty(q) ((q)->head == NULL)
#define sq_peek(q) ((q)->head)
#define dq_peek(q) ((q)->head)
/************************************************************************
* Global Type Declarations
************************************************************************/
struct sq_entry_s
{
FAR struct sq_entry_s *flink;
};
typedef struct sq_entry_s sq_entry_t;
struct dq_entry_s
{
FAR struct dq_entry_s *flink;
FAR struct dq_entry_s *blink;
};
typedef struct dq_entry_s dq_entry_t;
struct sq_queue_s
{
FAR sq_entry_t *head;
FAR sq_entry_t *tail;
};
typedef struct sq_queue_s sq_queue_t;
struct dq_queue_s
{
FAR dq_entry_t *head;
FAR dq_entry_t *tail;
};
typedef struct dq_queue_s dq_queue_t;
/************************************************************************
* Global Function Prototypes
************************************************************************/
#ifdef __cplusplus
#define EXTERN extern "C"
extern "C" {
#else
#define EXTERN extern
#endif
EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
sq_queue_t *queue);
EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
dq_queue_t *queue);
EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
#undef EXTERN
#ifdef __cplusplus
}
#endif
#endif /* __INCLUDE_QUEUE_H_ */
makefiles/config_px4fmu-v1_backside.mk
-148
View File
@@ -1,148 +0,0 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/eeprom
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
#
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/att_pos_estimator_ekf
#
# Vehicle Control
#
MODULES += modules/fixedwing_backside
#
# Logging
#
MODULES += modules/sdlog2
#
# Unit tests
#
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
#
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
#MODULES += examples/px4_daemon_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
#MODULES += examples/fixedwing_control
# Hardware test
#MODULES += examples/hwtest
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main ) \
$(call _B, sysinfo, , 2048, sysinfo_main )
makefiles/config_px4fmu-v1_default.mk
+1 -2
View File
@@ -42,8 +42,7 @@ MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/eeprom
MODULES += systemcmds/ramtron
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
makefiles/config_px4fmu-v1_test.mk
-48
View File
@@ -1,48 +0,0 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/px4io
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main )
makefiles/config_px4fmu-v2_default.mk
+1 -1
View File
@@ -46,7 +46,6 @@ MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/mixer
@@ -60,6 +59,7 @@ MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
#
# General system control
makefiles/config_px4fmu-v2_test.mk
+1
View File
@@ -22,6 +22,7 @@ MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
#
# Library modules
nuttx-configs/px4fmu-v1/nsh/defconfig
+20 -1
View File
@@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1
CONFIG_MMCSD_SPI=y
CONFIG_MMCSD_SPICLOCK=24000000
# CONFIG_MMCSD_SDIO is not set
# CONFIG_MTD is not set
CONFIG_MTD=y
CONFIG_PIPES=y
# CONFIG_PM is not set
# CONFIG_POWER is not set
@@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
# CONFIG_USART6_SERIAL_CONSOLE is not set
# CONFIG_NO_SERIAL_CONSOLE is not set
#
# MTD Configuration
#
CONFIG_MTD_PARTITION=y
CONFIG_MTD_BYTE_WRITE=y
#
# MTD Device Drivers
#
# CONFIG_RAMMTD is not set
# CONFIG_MTD_AT24XX is not set
# CONFIG_MTD_AT45DB is not set
# CONFIG_MTD_M25P is not set
# CONFIG_MTD_SMART is not set
# CONFIG_MTD_RAMTRON is not set
# CONFIG_MTD_SST25 is not set
# CONFIG_MTD_SST39FV is not set
# CONFIG_MTD_W25 is not set
#
# USART1 Configuration
#
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -500,8 +500,8 @@ CONFIG_MTD=y
#
# MTD Configuration
#
# CONFIG_MTD_PARTITION is not set
# CONFIG_MTD_BYTE_WRITE is not set
CONFIG_MTD_PARTITION=y
CONFIG_MTD_BYTE_WRITE=y
#
# MTD Device Drivers
src/drivers/boards/px4fmu-v2/board_config.h
+3 -3
View File
@@ -75,7 +75,7 @@ __BEGIN_DECLS
/* PX4FMU GPIOs ***********************************************************************************/
/* 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 */
#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_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_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 */
#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_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_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 */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
src/drivers/boards/px4io-v1/board_config.h
+3 -3
View File
@@ -58,11 +58,11 @@
/* PX4IO GPIOs **********************************************************************/
/* 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)
#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)
#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)
/* Safety switch button *************************************************************/
src/drivers/boards/px4io-v2/board_config.h
+3 -3
View File
@@ -74,9 +74,9 @@
/* LEDS **********************************************************************/
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|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_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#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_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
/* Safety switch button *******************************************************/
src/drivers/gps/gps.cpp
+132 -88
View File
@@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev
{
public:
GPS(const char *uart_path);
GPS(const char *uart_path, bool fake_gps);
virtual ~GPS();
virtual int init();
@@ -112,6 +112,7 @@ private:
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
/**
@@ -156,7 +157,7 @@ GPS *g_dev;
}
GPS::GPS(const char *uart_path) :
GPS::GPS(const char *uart_path, bool fake_gps) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
@@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) :
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
_report_pub(-1),
_rate(0.0f)
_rate(0.0f),
_fake_gps(fake_gps)
{
/* store port name */
strncpy(_port, uart_path, sizeof(_port));
@@ -264,98 +266,133 @@ GPS::task_main()
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_Helper != nullptr) {
delete(_Helper);
/* set to zero to ensure parser is not used while not instantiated */
_Helper = nullptr;
}
if (_fake_gps) {
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
_report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)400e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 10.0f;
_report.epv_m = 10.0f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
_report.vel_d_m_s = 0.0f;
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = 0.0f;
_report.vel_ned_valid = true;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
//no time and satellite information simulated
default:
break;
}
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
last_rate_count++;
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_Helper->store_update_rates();
_Helper->reset_update_rates();
}
if (!_healthy) {
char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
mode_str = "UBX";
break;
case GPS_DRIVER_MODE_MTK:
mode_str = "MTK";
break;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
if (_healthy) {
warnx("module lost");
_healthy = false;
_rate = 0.0f;
usleep(2e5);
} else {
if (_Helper != nullptr) {
delete(_Helper);
/* set to zero to ensure parser is not used while not instantiated */
_Helper = nullptr;
}
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
default:
break;
}
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
last_rate_count++;
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_Helper->store_update_rates();
_Helper->reset_update_rates();
}
if (!_healthy) {
char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
mode_str = "UBX";
break;
case GPS_DRIVER_MODE_MTK:
mode_str = "MTK";
break;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_healthy = false;
_rate = 0.0f;
}
lock();
}
lock();
}
lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
default:
break;
default:
break;
}
}
}
@@ -417,7 +454,7 @@ namespace gps
GPS *g_dev;
void start(const char *uart_path);
void start(const char *uart_path, bool fake_gps);
void stop();
void test();
void reset();
@@ -427,7 +464,7 @@ void info();
* Start the driver.
*/
void
start(const char *uart_path)
start(const char *uart_path, bool fake_gps)
{
int fd;
@@ -435,7 +472,7 @@ start(const char *uart_path)
errx(1, "already started");
/* create the driver */
g_dev = new GPS(uart_path);
g_dev = new GPS(uart_path, fake_gps);
if (g_dev == nullptr)
goto fail;
@@ -527,6 +564,7 @@ gps_main(int argc, char *argv[])
/* set to default */
char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
/*
* Start/load the driver.
@@ -542,7 +580,13 @@ gps_main(int argc, char *argv[])
}
}
gps::start(device_name);
/* Detect fake gps option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f"))
fake_gps = true;
}
gps::start(device_name, fake_gps);
}
if (!strcmp(argv[1], "stop"))
@@ -567,5 +611,5 @@ gps_main(int argc, char *argv[])
gps::info();
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
}
src/drivers/hil/hil.cpp
+5 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -193,9 +193,10 @@ HIL::~HIL()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// XXX already claimed with CDEV
// /* clean up the alternate device node */
// if (_primary_pwm_device)
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_hil = nullptr;
}
src/drivers/px4fmu/fmu.cpp
+11
View File
@@ -1108,10 +1108,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_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_ACCEL_MAG_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_MISO_OFF);
@@ -1124,10 +1126,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_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_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */
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_BARO, 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
}
src/drivers/px4fmu/module.mk
+1 -2
View File
@@ -3,5 +3,4 @@
#
MODULE_COMMAND = fmu
SRCS = fmu.cpp \
../../modules/systemlib/pwm_limit/pwm_limit.c
SRCS = fmu.cpp
src/drivers/rgbled/rgbled.cpp
+9 -3
View File
@@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
void
rgbled_usage()
{
warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
warnx(" -a addr (0x%x)", ADDR);
@@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[])
if (g_rgbled == nullptr) {
warnx("not started");
rgbled_usage();
exit(0);
exit(1);
}
if (!strcmp(verb, "test")) {
@@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(verb, "off")) {
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
@@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[])
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
src/modules/commander/commander.cpp
+19 -46
View File
@@ -106,14 +106,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
unsigned low_voltage_counter = 0;
unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -888,14 +880,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v;
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -948,46 +938,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
low_voltage_counter++;
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
status_changed = true;
}
/* End battery voltage check */
src/modules/commander/commander_helper.cpp
+28 -16
View File
@@ -44,6 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
float battery_remaining_estimate_voltage(float voltage)
float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
static unsigned int counter = 0;
static float ncells = 3;
// XXX change cells to int (and param to INT32)
if (!initialized) {
bat_volt_empty = param_find("BAT_V_EMPTY");
bat_volt_full = param_find("BAT_V_FULL");
bat_n_cells = param_find("BAT_N_CELLS");
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
initialized = true;
}
static float chemistry_voltage_empty = 3.2f;
static float chemistry_voltage_full = 4.05f;
if (counter % 100 == 0) {
param_get(bat_volt_empty, &chemistry_voltage_empty);
param_get(bat_volt_full, &chemistry_voltage_full);
param_get(bat_n_cells, &ncells);
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
}
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
src/modules/commander/commander_helper.h
+4 -3
View File
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
/**
* Provides a coarse estimate of remaining battery power.
* Estimate remaining battery charge.
*
* The estimate is very basic and based on decharging voltage curves.
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
float battery_remaining_estimate_voltage(float voltage, float discharged);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+4 -3
View File
@@ -48,6 +48,7 @@
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+60 -7
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 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
@@ -49,21 +48,75 @@
*
*/
/**
* L1 period
*
* This is the L1 distance and defines the tracking
* point ahead of the aircraft its following.
* A value of 25 meters works for most aircraft. Shorten
* slowly during tuning until response is sharp without oscillation.
*
* @min 1.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
/**
* L1 damping
*
* Damping factor for L1 control.
*
* @min 0.6
* @max 0.9
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
/**
* Default Loiter Radius
*
* This radius is used when no other loiter radius is set.
*
* @min 10.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
/**
* Cruise throttle
*
* This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
* Negative pitch limit
*
* The minimum negative pitch the controller will output.
*
* @unit degrees
* @min -60.0
* @max 0.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
/**
* Positive pitch limit
*
* The maximum positive pitch the controller will output.
*
* @unit degrees
* @min 0.0
* @max 60.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
src/modules/mavlink/mavlink.c
+2 -2
View File
@@ -680,8 +680,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_health,
v_status.load * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.battery_current * 100.0f,
v_status.battery_remaining * 100.0f,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
src/modules/sdlog2/sdlog2.c
+24 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -704,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -729,6 +730,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -755,6 +757,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -763,9 +766,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */
const ssize_t fdsc = 20;
/* sanity check variable and index */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -890,6 +893,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY --- */
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.battery_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -984,8 +993,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1251,6 +1258,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+11 -3
View File
@@ -148,8 +148,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -247,6 +245,15 @@ struct log_GVSP_s {
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float discharged;
};
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -280,7 +287,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -290,6 +297,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
src/modules/sensors/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# 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
src/modules/sensors/sensor_params.c
+172 -15
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* 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
@@ -38,6 +35,10 @@
* @file sensor_params.c
*
* Parameters defined by the sensors task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include <nuttx/config.h>
@@ -45,41 +46,98 @@
#include <systemlib/param/param.h>
/**
* Gyro X offset FIXME
* Gyro X offset
*
* This is an X-axis offset for the gyro.
* Adjust it according to the calibration data.
* This is an X-axis offset for the gyro. Adjust it according to the calibration data.
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
* Gyro Y offset FIXME with dot.
* Gyro Y offset
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
* Gyro Z offset FIXME
* Gyro Z offset
*
* @min -5.0
* @max 5.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
/**
* Gyro X scaling
*
* X-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
/**
* Gyro Y scaling
*
* Y-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
/**
* Gyro Z scaling
*
* Z-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
/**
* Magnetometer X offset
*
* This is an X-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
/**
* Magnetometer Y offset
*
* This is an Y-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
/**
* Magnetometer Z offset
*
* This is an Z-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
@@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* RC Channel 1 Minimum
*
* Minimum value for RC channel 1
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
/**
* RC Channel 1 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
/**
* RC Channel 1 Maximum
*
* Maximum value for RC channel 1
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
/**
* RC Channel 1 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
/**
* RC Channel 1 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
/**
* RC Channel 2 Minimum
*
* Minimum value for RC channel 2
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
/**
* RC Channel 2 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
/**
* RC Channel 2 Maximum
*
* Maximum value for RC channel 2
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
/**
* RC Channel 2 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
/**
* RC Channel 2 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
@@ -223,6 +379,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
src/modules/sensors/sensors.cpp
+62 -27
View File
@@ -114,6 +114,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -124,10 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -314,6 +317,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -467,7 +471,9 @@ Sensors::Sensors() :
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -560,6 +566,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -740,6 +747,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -1157,17 +1169,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
if (ret >= (int)sizeof(buf_adc[0])) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1178,27 +1189,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
_battery_status.voltage_v = voltage;
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1214,7 +1238,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1227,8 +1251,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
_last_adc = hrt_absolute_time();
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
}
@@ -1516,7 +1548,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = 0.0f;
_battery_status.voltage_filtered_v = 0.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
src/modules/systemlib/bson/tinybson.c
+3
View File
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
memcpy(encoder->buf, &len, sizeof(len));
}
/* sync file */
fsync(encoder->fd);
return 0;
}
src/modules/systemlib/module.mk
+2 -1
View File
@@ -51,5 +51,6 @@ SRCS = err.c \
mavlink_log.c \
rc_check.c \
otp.c \
board_serial.c
board_serial.c \
pwm_limit/pwm_limit.c
src/modules/systemlib/param/param.c
+29 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -61,7 +61,7 @@
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#if 1
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
# define debug(fmt, args...) do { } while(0)
@@ -512,6 +512,28 @@ param_save_default(void)
int fd;
const char *filename = param_get_default_file();
/* write parameters to temp file */
fd = open(filename, O_WRONLY | O_CREAT);
if (fd < 0) {
warn("failed to open param file: %s", filename);
return ERROR;
}
if (res == OK) {
res = param_export(fd, false);
if (res != OK) {
warnx("failed to write parameters to file: %s", filename);
}
}
close(fd);
return res;
#if 0
const char *filename_tmp = malloc(strlen(filename) + 5);
sprintf(filename_tmp, "%s.tmp", filename);
@@ -565,6 +587,7 @@ param_save_default(void)
free(filename_tmp);
return res;
#endif
}
/**
@@ -573,9 +596,9 @@ param_save_default(void)
int
param_load_default(void)
{
int fd = open(param_get_default_file(), O_RDONLY);
int fd_load = open(param_get_default_file(), O_RDONLY);
if (fd < 0) {
if (fd_load < 0) {
/* no parameter file is OK, otherwise this is an error */
if (errno != ENOENT) {
warn("open '%s' for reading failed", param_get_default_file());
@@ -584,8 +607,8 @@ param_load_default(void)
return 1;
}
int result = param_load(fd);
close(fd);
int result = param_load(fd_load);
close(fd_load);
if (result != 0) {
warn("error reading parameters from '%s'", param_get_default_file());
src/modules/systemlib/pwm_limit/pwm_limit.c
+64 -46
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
#include <math.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
void pwm_limit_init(pwm_limit_t *limit)
{
limit->state = LIMIT_STATE_OFF;
limit->state = PWM_LIMIT_STATE_INIT;
limit->time_armed = 0;
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 */
switch (limit->state) {
case LIMIT_STATE_OFF:
if (armed)
limit->state = LIMIT_STATE_RAMP;
limit->time_armed = hrt_absolute_time();
case PWM_LIMIT_STATE_INIT:
if (armed) {
/* 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;
case LIMIT_STATE_INIT:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
limit->state = LIMIT_STATE_RAMP;
case PWM_LIMIT_STATE_OFF:
if (armed) {
limit->state = PWM_LIMIT_STATE_RAMP;
/* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time();
}
break;
case LIMIT_STATE_RAMP:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
limit->state = LIMIT_STATE_ON;
case PWM_LIMIT_STATE_RAMP:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON;
}
break;
case LIMIT_STATE_ON:
if (!armed)
limit->state = LIMIT_STATE_OFF;
case PWM_LIMIT_STATE_ON:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
}
break;
default:
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 */
switch (limit->state) {
case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT:
case PWM_LIMIT_STATE_OFF:
case PWM_LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
}
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;
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
progress = diff * 10000 / RAMP_TIME_US;
/* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
/* safeguard against overflows */
unsigned disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i];
}
unsigned disarmed_min_diff = min_pwm[i] - disarmed;
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;
case LIMIT_STATE_ON:
case PWM_LIMIT_STATE_ON:
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_output stays the same */
}
break;
default:
src/modules/systemlib/pwm_limit/pwm_limit.h
+11 -9
View File
@@ -46,6 +46,8 @@
#include <stdint.h>
#include <stdbool.h>
__BEGIN_DECLS
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
@@ -56,21 +58,21 @@
*/
#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 {
enum {
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
enum pwm_limit_state state;
uint64_t time_armed;
} pwm_limit_t;
__BEGIN_DECLS
__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
src/modules/uORB/topics/battery_status.h
+5 -4
View File
@@ -53,9 +53,10 @@
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float voltage_v; /**< Battery voltage in volts, filtered */
float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
float voltage_v; /**< Battery voltage in volts, 0 if unknown */
float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */
float current_a; /**< Battery current in amperes, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**
@@ -65,4 +66,4 @@ struct battery_status_s {
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
#endif
#endif
src/systemcmds/eeprom/eeprom.c
-265
View File
@@ -1,265 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file eeprom.c
*
* EEPROM service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <board_config.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
#ifndef PX4_I2C_BUS_ONBOARD
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
#endif
__EXPORT int eeprom_main(int argc, char *argv[]);
static void eeprom_attach(void);
static void eeprom_start(void);
static void eeprom_erase(void);
static void eeprom_ioctl(unsigned operation);
static void eeprom_save(const char *name);
static void eeprom_load(const char *name);
static void eeprom_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *eeprom_mtd;
int eeprom_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
eeprom_start();
if (!strcmp(argv[1], "save_param"))
eeprom_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
eeprom_load(argv[2]);
if (!strcmp(argv[1], "erase"))
eeprom_erase();
if (!strcmp(argv[1], "test"))
eeprom_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
eeprom_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
eeprom_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
}
static void
eeprom_attach(void)
{
/* find the right I2C */
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
/* this resets the I2C bus, set correct bus speed again */
I2C_SETFREQUENCY(i2c, 400000);
if (i2c == NULL)
errx(1, "failed to locate I2C bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
eeprom_mtd = at24c_initialize(i2c);
if (eeprom_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: EEPROM needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (eeprom_mtd == NULL)
errx(1, "failed to initialize EEPROM driver");
attached = true;
}
static void
eeprom_start(void)
{
int ret;
if (started)
errx(1, "EEPROM already mounted");
if (!attached)
eeprom_attach();
/* start NXFFS */
ret = nxffs_initialize(eeprom_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
/* mount the EEPROM */
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
started = true;
warnx("mounted EEPROM at /eeprom");
exit(0);
}
extern int at24c_nuke(void);
static void
eeprom_erase(void)
{
if (!attached)
eeprom_attach();
if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
eeprom_ioctl(unsigned operation)
{
int fd;
fd = open("/eeprom/.", 0);
if (fd < 0)
err(1, "open /eeprom");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
eeprom_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
eeprom_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
extern void at24c_test(void);
static void
eeprom_test(void)
{
at24c_test();
exit(0);
}
src/systemcmds/eeprom/module.mk
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# EEPROM file system driver
#
MODULE_COMMAND = eeprom
SRCS = 24xxxx_mtd.c eeprom.c
src/systemcmds/eeprom/24xxxx_mtd.c → src/systemcmds/mtd/24xxxx_mtd.c
View File
src/systemcmds/mtd/module.mk
+6
View File
@@ -0,0 +1,6 @@
#
# RAMTRON file system driver
#
MODULE_COMMAND = mtd
SRCS = mtd.c 24xxxx_mtd.c
src/systemcmds/mtd/mtd.c
+493
View File
@@ -0,0 +1,493 @@
/****************************************************************************
*
* Copyright (c) 2013-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 mtd.c
*
* mtd service and utility app.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#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"
#include <board_config.h>
__EXPORT int mtd_main(int argc, char *argv[]);
#ifndef CONFIG_MTD
/* create a fake command with decent warnx to not confuse users */
int mtd_main(int argc, char *argv[])
{
errx(1, "MTD not enabled, skipping.");
}
#else
#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_test(void);
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 started = false;
static struct mtd_dev_s *mtd_dev;
static unsigned n_partitions_current = 0;
/* note, these will be equally sized */
static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]);
int mtd_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start")) {
/* start mapping according to user request */
if (argc >= 3) {
mtd_start(argv + 2, argc - 2);
} else {
mtd_start(partition_names_default, n_partitions_default);
}
}
if (!strcmp(argv[1], "test"))
mtd_test();
if (!strcmp(argv[1], "readtest")) {
if (argc >= 3) {
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);
}
}
}
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 *mtd_partition(FAR struct mtd_dev_s *mtd,
off_t firstblock, off_t nblocks);
#ifdef CONFIG_MTD_RAMTRON
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 */
SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
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 RAMTRON driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
mtd_dev = ramtron_initialize(spi);
if (mtd_dev) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: mtd needed %d attempts to attach", i + 1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (mtd_dev == NULL)
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;
}
#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
mtd_start(char *partition_names[], unsigned n_partitions)
{
int ret;
if (started)
errx(1, "mtd already mounted");
if (!attached) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
at24xxx_attach();
#else
ramtron_attach();
#endif
}
if (!mtd_dev) {
warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
exit(1);
}
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize;
ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
if (ret)
exit(3);
/* Now create MTD FLASH partitions */
warnx("Creating partitions");
FAR struct mtd_dev_s *part[n_partitions];
char blockname[32];
unsigned offset;
unsigned i;
for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
warnx(" Partition %d. Block offset=%lu, size=%lu",
i, (unsigned long)offset, (unsigned long)nblocks);
/* Create the partition */
part[i] = mtd_partition(mtd_dev, offset, nblocks);
if (!part[i]) {
warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu",
(unsigned long)offset, (unsigned long)nblocks);
exit(4);
}
/* Initialize to provide an FTL block driver on the MTD FLASH interface */
snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i);
ret = ftl_initialize(i, part[i]);
if (ret < 0) {
warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret);
exit(5);
}
/* Now create a character device on the block device */
ret = bchdev_register(blockname, partition_names[i], false);
if (ret < 0) {
warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret);
exit(6);
}
}
n_partitions_current = n_partitions;
started = true;
exit(0);
}
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)
{
warnx("This test routine does not test anything yet!");
exit(1);
}
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)
{
uint8_t v[64];
memset(v, 0xFF, sizeof(v));
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
printf("Erasing %s\n", partition_names[i]);
int fd = open(partition_names[i], O_WRONLY);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (write(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
}
printf("Erased %lu bytes\n", (unsigned long)count);
close(fd);
}
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
src/systemcmds/param/param.c
+38 -9
View File
@@ -72,7 +72,12 @@ param_main(int argc, char *argv[])
if (argc >= 3) {
do_save(argv[2]);
} else {
do_save(param_get_default_file());
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}
}
@@ -133,11 +138,8 @@ param_main(int argc, char *argv[])
static void
do_save(const char* param_file_name)
{
/* delete the parameter file in case it exists */
unlink(param_file_name);
/* create the file */
int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
int fd = open(param_file_name, O_WRONLY | O_CREAT);
if (fd < 0)
err(1, "opening '%s' failed", param_file_name);
@@ -146,7 +148,7 @@ do_save(const char* param_file_name)
close(fd);
if (result < 0) {
unlink(param_file_name);
(void)unlink(param_file_name);
errx(1, "error exporting to '%s'", param_file_name);
}
@@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param)
int32_t i;
float f;
const char *search_string = (const char*)arg;
const char *p_name = (const char*)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
/* param not found */
return;
if (!(arg == NULL)) {
/* start search */
char *ss = search_string;
char *pp = p_name;
bool mismatch = false;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
if (*ss == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
warnx("* symbol only allowed at end of search string.");
exit(1);
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*'))
return;
}
printf("%c %s: ",
src/systemcmds/ramtron/module.mk
-6
View File
@@ -1,6 +0,0 @@
#
# RAMTRON file system driver
#
MODULE_COMMAND = ramtron
SRCS = ramtron.c
src/systemcmds/ramtron/ramtron.c
-279
View File
@@ -1,279 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ramtron.c
*
* ramtron service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/spi.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int ramtron_main(int argc, char *argv[]);
#ifndef CONFIG_MTD_RAMTRON
/* create a fake command with decent message to not confuse users */
int ramtron_main(int argc, char *argv[])
{
errx(1, "RAMTRON not enabled, skipping.");
}
#else
static void ramtron_attach(void);
static void ramtron_start(void);
static void ramtron_erase(void);
static void ramtron_ioctl(unsigned operation);
static void ramtron_save(const char *name);
static void ramtron_load(const char *name);
static void ramtron_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *ramtron_mtd;
int ramtron_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
ramtron_start();
if (!strcmp(argv[1], "save_param"))
ramtron_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
ramtron_load(argv[2]);
if (!strcmp(argv[1], "erase"))
ramtron_erase();
if (!strcmp(argv[1], "test"))
ramtron_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
ramtron_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
ramtron_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
}
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
static void
ramtron_attach(void)
{
/* find the right spi */
struct spi_dev_s *spi = up_spiinitialize(2);
/* this resets the spi bus, set correct bus speed again */
// xxx set in ramtron driver, leave this out
// SPI_SETFREQUENCY(spi, 4000000);
SPI_SETFREQUENCY(spi, 375000000);
SPI_SETBITS(spi, 8);
SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false);
if (spi == NULL)
errx(1, "failed to locate spi bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
ramtron_mtd = ramtron_initialize(spi);
if (ramtron_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: ramtron needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (ramtron_mtd == NULL)
errx(1, "failed to initialize ramtron driver");
attached = true;
}
static void
ramtron_start(void)
{
int ret;
if (started)
errx(1, "ramtron already mounted");
if (!attached)
ramtron_attach();
/* start NXFFS */
ret = nxffs_initialize(ramtron_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
/* mount the ramtron */
ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /ramtron - erase ramtron to reformat");
started = true;
warnx("mounted ramtron at /ramtron");
exit(0);
}
//extern int at24c_nuke(void);
static void
ramtron_erase(void)
{
if (!attached)
ramtron_attach();
// if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
ramtron_ioctl(unsigned operation)
{
int fd;
fd = open("/ramtron/.", 0);
if (fd < 0)
err(1, "open /ramtron");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
ramtron_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
ramtron_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
//extern void at24c_test(void);
static void
ramtron_test(void)
{
// at24c_test();
exit(0);
}
#endif
src/systemcmds/tests/module.mk
+3 -1
View File
@@ -29,4 +29,6 @@ SRCS = test_adc.c \
test_param.c \
test_ppm_loopback.c \
test_rc.c \
test_mount.c
test_conv.cpp \
test_mount.c \
test_mtd.c
src/systemcmds/tests/test_conv.cpp
+76
View File
@@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_conv.cpp
* Tests conversions used across the system.
*
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests.h"
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <unit_test/unit_test.h>
#include <px4iofirmware/protocol.h>
int test_conv(int argc, char *argv[])
{
warnx("Testing system conversions");
for (int i = -10000; i <= 10000; i+=1) {
float f = i/10000.0f;
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
if (fabsf(f - fres) > 0.0001f) {
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
return 1;
}
}
warnx("All conversions clean");
return 0;
}
src/systemcmds/tests/test_file.c
+9 -7
View File
@@ -54,9 +54,9 @@
#include "tests.h"
int check_user_abort();
static int check_user_abort(int fd);
int check_user_abort() {
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
@@ -77,6 +77,8 @@ int check_user_abort() {
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
@@ -141,7 +143,7 @@ test_file(int argc, char *argv[])
fsync(fd);
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -175,7 +177,7 @@ test_file(int argc, char *argv[])
return 1;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -199,7 +201,7 @@ test_file(int argc, char *argv[])
return 1;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -232,7 +234,7 @@ test_file(int argc, char *argv[])
break;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -275,7 +277,7 @@ test_file(int argc, char *argv[])
break;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
src/systemcmds/tests/test_mixer.cpp
+195 -2
View File
@@ -48,9 +48,13 @@
#include <errno.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include <systemlib/err.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"
@@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index,
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[])
{
warnx("testing mixer");
@@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
if (mixer_group.count() != 8)
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 */
mixer_group.reset();
@@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[])
unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded);
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;
}
warnx("SUCCESS: No errors in mixer test");
}
static int
@@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
if (control_group != 0)
return -1;
control = 0.0f;
if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
return -1;
control = actuator_controls[control_index];
return 0;
}
src/systemcmds/tests/test_mtd.c
+229
View File
@@ -0,0 +1,229 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_mtd.c
*
* Param storage / file test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <poll.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
#define PARAM_FILE_NAME "/fs/mtd_params"
static int check_user_abort(int fd);
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
}
}
return 1;
}
void print_fail()
{
printf("<[T]: MTD: FAIL>\n");
}
void print_success()
{
printf("<[T]: MTD: OK>\n");
}
int
test_mtd(int argc, char *argv[])
{
unsigned iterations= 0;
/* check if microSD card is mounted */
struct stat buffer;
if (stat(PARAM_FILE_NAME, &buffer)) {
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
print_fail();
return 1;
}
// XXX get real storage space here
unsigned file_size = 4096;
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {256, 512, 4096};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open(PARAM_FILE_NAME, O_RDONLY);
int rret = read(fd, read_buf, chunk_sizes[c]);
close(fd);
fd = open(PARAM_FILE_NAME, O_WRONLY);
printf("printing 2 percent of the first chunk:\n");
for (int i = 0; i < sizeof(read_buf) / 50; i++) {
printf("%02X", read_buf[i]);
}
printf("\n");
iterations = file_size / chunk_sizes[c];
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
print_fail();
return 1;
}
fsync(fd);
if (!check_user_abort(fd))
return OK;
}
end = hrt_absolute_time();
close(fd);
fd = open(PARAM_FILE_NAME, O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
warnx("READ ERROR!");
print_fail();
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d", j);
print_fail();
compare_ok = false;
break;
}
}
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
print_fail();
return 1;
}
if (!check_user_abort(fd))
return OK;
}
close(fd);
}
/* fill the file with 0xFF to make it look new again */
char ffbuf[64];
memset(ffbuf, 0xFF, sizeof(ffbuf));
int fd = open(PARAM_FILE_NAME, O_WRONLY);
for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
int ret = write(fd, ffbuf, sizeof(ffbuf));
if (ret != sizeof(ffbuf)) {
warnx("ERROR! Could not fill file with 0xFF");
close(fd);
print_fail();
return 1;
}
}
(void)close(fd);
print_success();
return 0;
}
src/systemcmds/tests/test_rc.c
+2 -2
View File
@@ -32,8 +32,8 @@
****************************************************************************/
/**
* @file test_ppm_loopback.c
* Tests the PWM outputs and PPM input
* @file test_rc.c
* Tests RC input.
*
*/
src/systemcmds/tests/tests.h
+2
View File
@@ -109,7 +109,9 @@ extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]);
extern int test_mixer(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_mtd(int argc, char *argv[]);
__END_DECLS
src/systemcmds/tests/tests_main.c
+2
View File
@@ -106,7 +106,9 @@ const struct {
{"file", test_file, 0},
{"mixer", test_mixer, 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},
{"mtd", test_mtd, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}
};