fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into mixer unit tests branch

Browse Source
This commit is contained in:
Lorenz Meier 2014-01-15 07:43:17 +01:00
commit f3cd83e804
69 changed files with 2886 additions and 1044 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ROMFS/px4fmu_common/init.d/rcS
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
@ -213,7 +214,9 @@ then
# 7000 .. 7999 Hexa +
# 8000 .. 8999 Octo X
# 9000 .. 9999 Octo +
# 10000 .. 19999 Wide arm / H frame
# 10000 .. 10999 Wide arm / H frame
# 11000 .. 11999 Hexa Cox
# 12000 .. 12999 Octo Cox
if param compare SYS_AUTOSTART 4008 8
then
@ -277,6 +280,13 @@ then
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 12001
then
set MIXER /etc/mixers/FMU_octo_cox.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 10015 15
then

67
ROMFS/px4fmu_test/init.d/rcS
View File

@ -2,6 +2,7 @@
#
# PX4FMU startup script for test hackery.
#
uorb start
if sercon
then
@ -9,4 +10,68 @@ then
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
fi
#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
echo "[init] no microSD card found"
# Play SOS
tone_alarm error
fi
#
# Start a minimal system
#
if [ -f /etc/extras/px4io-v2_default.bin ]
then
set io_file /etc/extras/px4io-v2_default.bin
else
set io_file /etc/extras/px4io-v1_default.bin
fi
if px4io start
then
echo "PX4IO OK"
fi
if px4io checkcrc $io_file
then
echo "PX4IO CRC OK"
else
echo "PX4IO CRC failure"
tone_alarm MBABGP
if px4io forceupdate 14662 $io_file
then
usleep 500000
if px4io start
then
echo "PX4IO restart OK"
tone_alarm MSPAA
else
echo "PX4IO restart failed"
tone_alarm MNGGG
sleep 5
reboot
fi
else
echo "PX4IO update failed"
tone_alarm MNGGG
fi
fi
#
# The presence of this file suggests we're running a mount stress test
#
if [ -f /fs/microsd/mount_test_cmds.txt ]
then
tests mount
fi

30
Tools/px4params/dokuwikiout.py
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

27
Tools/px4params/dokuwikiout_listings.py Normal file
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

18
makefiles/config_px4fmu-v1_backside.mk
View File

@ -25,14 +25,10 @@ MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
@ -42,8 +38,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
@ -71,22 +66,11 @@ MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
#MODULES += modules/attitude_estimator_ekf
MODULES += modules/att_pos_estimator_ekf
#MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
MODULES += modules/attitude_estimator_so3
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
#MODULES += modules/multirotor_att_control
#MODULES += modules/multirotor_pos_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
MODULES += modules/fixedwing_backside
#

14
makefiles/config_px4fmu-v1_default.mk
View File

@ -21,7 +21,7 @@ MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
#MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
@ -33,17 +33,16 @@ MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/eeprom
MODULES += systemcmds/ramtron
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
@ -75,18 +74,17 @@ MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
#MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += examples/flow_position_control
MODULES += examples/flow_speed_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
#
# Logging

49
makefiles/config_px4fmu-v1_test.mk Normal file
View File

@ -0,0 +1,49 @@
#
# 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
MODULES += systemcmds/mtd
#
# 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 )

3
makefiles/config_px4fmu-v2_default.mk
View File

@ -36,6 +36,7 @@ MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
# Needs to be burned to the ground and re-written; for now,
@ -45,7 +46,6 @@ MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/mixer
@ -59,6 +59,7 @@ MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
#
# General system control

8
makefiles/config_px4fmu-v2_test.mk
View File

@ -6,21 +6,29 @@
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_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-v2
MODULES += drivers/px4io
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
#

3
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs \
-Wunsuffixed-float-constants
-Wnested-externs
# C++-specific warnings
#

23
nuttx-configs/px4fmu-v1/nsh/defconfig
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
#
@ -566,7 +585,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=2048
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0010
CONFIG_CDCACM_VENDORSTR="3D Robotics"

20
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -295,16 +295,16 @@ CONFIG_STM32_USART=y
# U[S]ART Configuration
#
# CONFIG_USART1_RS485 is not set
# CONFIG_USART1_RXDMA is not set
CONFIG_USART1_RXDMA=y
# CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RS485 is not set
CONFIG_USART3_RXDMA=y
# CONFIG_UART4_RS485 is not set
CONFIG_UART4_RXDMA=y
# CONFIG_UART5_RXDMA is not set
CONFIG_UART5_RXDMA=y
# CONFIG_USART6_RS485 is not set
# CONFIG_USART6_RXDMA is not set
CONFIG_USART6_RXDMA=y
# CONFIG_UART7_RS485 is not set
# CONFIG_UART7_RXDMA is not set
# CONFIG_UART8_RS485 is not set
@ -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
@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y
#
# UART4 Configuration
#
CONFIG_UART4_RXBUFSIZE=128
CONFIG_UART4_TXBUFSIZE=128
CONFIG_UART4_RXBUFSIZE=512
CONFIG_UART4_TXBUFSIZE=512
CONFIG_UART4_BAUD=57600
CONFIG_UART4_BITS=8
CONFIG_UART4_PARITY=0
@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0
#
# USART6 Configuration
#
CONFIG_USART6_RXBUFSIZE=256
CONFIG_USART6_TXBUFSIZE=256
CONFIG_USART6_RXBUFSIZE=512
CONFIG_USART6_TXBUFSIZE=512
CONFIG_USART6_BAUD=57600
CONFIG_USART6_BITS=8
CONFIG_USART6_PARITY=0
@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=2048
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"

7
src/drivers/boards/px4fmu-v1/board_config.h
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
@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv1 internal definitions
*/
#pragma once
@ -60,6 +60,7 @@ __BEGIN_DECLS
/* PX4IO connection configuration */
#define PX4IO_SERIAL_DEVICE "/dev/ttyS2"
#define UDID_START 0x1FFF7A10
//#ifdef CONFIG_STM32_SPI2
//# error "SPI2 is not supported on this board"
@ -180,7 +181,7 @@ __BEGIN_DECLS
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/****************************************************************************************************
* Public Types

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv2 internal definitions
*/
#pragma once
@ -52,6 +52,8 @@ __BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32.h>
#include <arch/board/board.h>
#define UDID_START 0x1FFF7A10
/****************************************************************************************************
* Definitions
@ -82,21 +84,21 @@ __BEGIN_DECLS
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* Data ready pins off */
#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_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_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)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
/* SPI1 chip selects off */
#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_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_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)
@ -177,7 +179,7 @@ __BEGIN_DECLS
*
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
*/
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
/* High-resolution timer */
#define HRT_TIMER 8 /* use timer8 for the HRT */

15
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
View File

@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\n");
message("[boot] Initialized SPI port 1 (SENSORS)\n");
/* Get the SPI port for the FRAM */
@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void)
return -ENODEV;
}
/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 375000000);
/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
* and de-assert the known chip selects. */
// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
message("[boot] Successfully initialized SPI port 2\n");
message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void)
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}

4
src/drivers/boards/px4io-v1/board_config.h
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
@ -92,4 +92,4 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file px4iov2_internal.h
* @file board_config.h
*
* PX4IOV2 internal definitions
*/
@ -122,7 +122,7 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
/* LED definitions ******************************************************************/
/* PX4 has two LEDs that we will encode as: */

2
src/drivers/drv_rc_input.h
View File

@ -60,7 +60,7 @@
/**
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
#define RC_INPUT_MAX_CHANNELS 20
#define RC_INPUT_MAX_CHANNELS 18
/**
* Input signal type, value is a control position from zero to 100

289
src/drivers/frsky_telemetry/frsky_data.c Normal file
View File

@ -0,0 +1,289 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* 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 frsky_data.c
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
*/
#include "frsky_data.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
/* FrSky sensor hub data IDs */
#define FRSKY_ID_GPS_ALT_BP 0x01
#define FRSKY_ID_TEMP1 0x02
#define FRSKY_ID_RPM 0x03
#define FRSKY_ID_FUEL 0x04
#define FRSKY_ID_TEMP2 0x05
#define FRSKY_ID_VOLTS 0x06
#define FRSKY_ID_GPS_ALT_AP 0x09
#define FRSKY_ID_BARO_ALT_BP 0x10
#define FRSKY_ID_GPS_SPEED_BP 0x11
#define FRSKY_ID_GPS_LONG_BP 0x12
#define FRSKY_ID_GPS_LAT_BP 0x13
#define FRSKY_ID_GPS_COURS_BP 0x14
#define FRSKY_ID_GPS_DAY_MONTH 0x15
#define FRSKY_ID_GPS_YEAR 0x16
#define FRSKY_ID_GPS_HOUR_MIN 0x17
#define FRSKY_ID_GPS_SEC 0x18
#define FRSKY_ID_GPS_SPEED_AP 0x19
#define FRSKY_ID_GPS_LONG_AP 0x1A
#define FRSKY_ID_GPS_LAT_AP 0x1B
#define FRSKY_ID_GPS_COURS_AP 0x1C
#define FRSKY_ID_BARO_ALT_AP 0x21
#define FRSKY_ID_GPS_LONG_EW 0x22
#define FRSKY_ID_GPS_LAT_NS 0x23
#define FRSKY_ID_ACCEL_X 0x24
#define FRSKY_ID_ACCEL_Y 0x25
#define FRSKY_ID_ACCEL_Z 0x26
#define FRSKY_ID_CURRENT 0x28
#define FRSKY_ID_VARIO 0x30
#define FRSKY_ID_VFAS 0x39
#define FRSKY_ID_VOLTS_BP 0x3A
#define FRSKY_ID_VOLTS_AP 0x3B
#define frac(f) (f - (int)f)
static int battery_sub = -1;
static int sensor_sub = -1;
static int global_position_sub = -1;
static int vehicle_status_sub = -1;
/**
* Initializes the uORB subscriptions.
*/
void frsky_init()
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
/**
* Sends a 0x5E start/stop byte.
*/
static void frsky_send_startstop(int uart)
{
static const uint8_t c = 0x5E;
write(uart, &c, sizeof(c));
}
/**
* Sends one byte, performing byte-stuffing if necessary.
*/
static void frsky_send_byte(int uart, uint8_t value)
{
const uint8_t x5E[] = { 0x5D, 0x3E };
const uint8_t x5D[] = { 0x5D, 0x3D };
switch (value) {
case 0x5E:
write(uart, x5E, sizeof(x5E));
break;
case 0x5D:
write(uart, x5D, sizeof(x5D));
break;
default:
write(uart, &value, sizeof(value));
break;
}
}
/**
* Sends one data id/value pair.
*/
static void frsky_send_data(int uart, uint8_t id, int16_t data)
{
/* Cast data to unsigned, because signed shift might behave incorrectly */
uint16_t udata = data;
frsky_send_startstop(uart);
frsky_send_byte(uart, id);
frsky_send_byte(uart, udata); /* LSB */
frsky_send_byte(uart, udata >> 8); /* MSB */
}
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(raw.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
roundf(raw.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
raw.baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(raw.baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(raw.baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery.voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
frsky_send_startstop(uart);
}
/**
* Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
*/
static float frsky_format_gps(float dec)
{
float dms_deg = (int) dec;
float dec_deg = dec - dms_deg;
float dms_min = (int) (dec_deg * 60);
float dec_min = (dec_deg * 60) - dms_min;
float dms_sec = dec_min * 60;
return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
}
/**
* Sends frame 2 (every 1000ms):
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
*/
void frsky_send_frame2(int uart)
{
/* get a local copy of the global position data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos.valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
* 25.0f / 46.0f;
alt = global_pos.alt;
sec = tm_gps->tm_sec;
}
frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(vehicle_status.battery_remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
frsky_send_startstop(uart);
}
/**
* Sends frame 3 (every 5000ms):
* GPS date & time
*/
void frsky_send_frame3(int uart)
{
/* get a local copy of the battery data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* send formatted frame */
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
frsky_send_startstop(uart);
}

51
src/drivers/frsky_telemetry/frsky_data.h Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* 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 frsky_data.h
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
*/
#ifndef _FRSKY_DATA_H
#define _FRSKY_DATA_H
// Public functions
void frsky_init(void);
void frsky_send_frame1(int uart);
void frsky_send_frame2(int uart);
void frsky_send_frame3(int uart);
#endif /* _FRSKY_TELEMETRY_H */

266
src/drivers/frsky_telemetry/frsky_telemetry.c Normal file
View File

@ -0,0 +1,266 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* 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 frsky_telemetry.c
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
* This daemon emulates an FrSky sensor hub by periodically sending data
* packets to an attached FrSky receiver.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <termios.h>
#include "frsky_data.h"
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static int frsky_task;
/* functions */
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
static void usage(void);
static int frsky_telemetry_thread_main(int argc, char *argv[]);
__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
/**
* Opens the UART device and sets all required serial parameters.
*/
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
{
/* Open UART */
const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", uart_name);
}
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config);
/* Disable output post-processing */
uart_config.c_oflag &= ~OPOST;
/* Set baud rate */
static const speed_t speed = B9600;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
return uart;
}
/**
* Print command usage information
*/
static void usage()
{
fprintf(stderr,
"usage: frsky_telemetry start [-d <devicename>]\n"
" frsky_telemetry stop\n"
" frsky_telemetry status\n");
exit(1);
}
/**
* The daemon thread.
*/
static int frsky_telemetry_thread_main(int argc, char *argv[])
{
/* Default values for arguments */
char *device_name = "/dev/ttyS1"; /* USART2 */
/* Work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "d:")) != EOF) {
switch (ch) {
case 'd':
device_name = optarg;
break;
default:
usage();
break;
}
}
/* Print welcome text */
warnx("FrSky telemetry interface starting...");
/* Open UART */
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
if (uart < 0)
err(1, "could not open %s", device_name);
/* Subscribe to topics */
frsky_init();
thread_running = true;
/* Main thread loop */
unsigned int iteration = 0;
while (!thread_should_exit) {
/* Sleep 200 ms */
usleep(200000);
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
frsky_send_frame1(uart);
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0)
{
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0)
{
frsky_send_frame3(uart);
iteration = 0;
}
iteration++;
}
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
/**
* The main command function.
* Processes command line arguments and starts the daemon.
*/
int frsky_telemetry_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("missing command");
usage();
}
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running)
errx(0, "frsky_telemetry already running");
thread_should_exit = false;
frsky_task = task_spawn_cmd("frsky_telemetry",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
frsky_telemetry_thread_main,
(const char **)argv);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running)
errx(0, "frsky_telemetry already stopped");
thread_should_exit = true;
while (thread_running) {
usleep(200000);
warnx(".");
}
warnx("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
usage();
/* not getting here */
return 0;
}

10
src/systemcmds/eeprom/module.mk → src/drivers/frsky_telemetry/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# 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
@ -32,8 +32,10 @@
############################################################################
#
# EEPROM file system driver
# FrSky telemetry application.
#
MODULE_COMMAND = eeprom
SRCS = 24xxxx_mtd.c eeprom.c
MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.c \
frsky_telemetry.c

220
src/drivers/gps/gps.cpp
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]");
}

9
src/drivers/hil/hil.cpp
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;
}

2
src/drivers/hott/hott_sensors/hott_sensors.cpp
View File

@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
SCHED_PRIORITY_DEFAULT,
1024,
hott_sensors_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);

2
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
View File

@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
SCHED_PRIORITY_DEFAULT,
2048,
hott_telemetry_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);

4
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -198,7 +198,9 @@ MEASAirspeed::collect()
// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
if (diff_press_pa < 0.0f)
diff_press_pa = 0.0f;
struct differential_pressure_s report;

6
src/drivers/ms5611/ms5611.cpp
View File

@ -124,6 +124,8 @@ protected:
int32_t _TEMP;
int64_t _OFF;
int64_t _SENS;
float _P;
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in kPa */
@ -623,6 +625,8 @@ MS5611::collect()
/* pressure calculation, result in Pa */
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
_P = P * 0.01f;
_T = _TEMP * 0.01f;
/* generate a new report */
report.temperature = _TEMP / 100.0f;
@ -695,6 +699,8 @@ MS5611::print_info()
printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF);
printf("P: %.3f\n", _P);
printf("T: %.3f\n", _T);
printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
printf("factory_setup %u\n", _prom.factory_setup);

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

@ -65,6 +65,7 @@
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/board_serial.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
@ -224,10 +225,10 @@ PX4FMU::PX4FMU() :
_armed(false),
_pwm_on(false),
_mixers(nullptr),
_failsafe_pwm( {0}),
_disarmed_pwm( {0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@ -575,7 +576,7 @@ PX4FMU::task_main()
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
/* FALLTHROUGH */
/* FALLTHROUGH */
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
if (_mode < MODE_4PWM) {
@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
/* FALLTHROUGH */
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
if (arg <= 2100) {
@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
/* FALLTHROUGH */
/* FALLTHROUGH */
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
if (_mode < MODE_4PWM) {
@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
/* FALLTHROUGH */
/* FALLTHROUGH */
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
@ -1107,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);
@ -1123,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);
@ -1159,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
}
@ -1591,6 +1603,15 @@ fmu_main(int argc, char *argv[])
errx(0, "FMU driver stopped");
}
if (!strcmp(verb, "id")) {
char id[12];
(void)get_board_serial(id);
errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
(unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
(unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
}
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
@ -1647,6 +1668,7 @@ fmu_main(int argc, char *argv[])
sensor_reset(0);
warnx("resettet default time");
}
exit(0);
}

116
src/drivers/px4io/px4io.cpp
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
@ -35,7 +35,7 @@
* @file px4io.cpp
* Driver for the PX4IO board.
*
* PX4IO is connected via I2C.
* PX4IO is connected via I2C or DMA enabled high-speed UART.
*/
#include <nuttx/config.h>
@ -1690,6 +1690,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
if (ret)
return ret;
retries--;
log("mixer sent");
@ -2419,6 +2422,69 @@ detect(int argc, char *argv[])
}
}
void
checkcrc(int argc, char *argv[])
{
bool keep_running = false;
if (g_dev == nullptr) {
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
errx(1, "driver alloc failed");
} else {
/* its already running, don't kill the driver */
keep_running = true;
}
/*
check IO CRC against CRC of a file
*/
if (argc < 2) {
printf("usage: px4io checkcrc filename\n");
exit(1);
}
int fd = open(argv[1], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[1], errno);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
uint32_t nbytes = 0;
while (true) {
uint8_t buf[16];
int n = read(fd, buf, sizeof(buf));
if (n <= 0) break;
fw_crc = crc32part(buf, n, fw_crc);
nbytes += n;
}
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
if (!keep_running) {
delete g_dev;
g_dev = nullptr;
}
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
}
printf("CRCs match\n");
exit(0);
}
void
bind(int argc, char *argv[])
{
@ -2613,6 +2679,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "detect"))
detect(argc - 1, argv + 1);
if (!strcmp(argv[1], "checkcrc"))
checkcrc(argc - 1, argv + 1);
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
@ -2798,49 +2867,6 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "checkcrc")) {
/*
check IO CRC against CRC of a file
*/
if (argc <= 2) {
printf("usage: px4io checkcrc filename\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
int fd = open(argv[2], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[2], errno);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
uint32_t nbytes = 0;
while (true) {
uint8_t buf[16];
int n = read(fd, buf, sizeof(buf));
if (n <= 0) break;
fw_crc = crc32part(buf, n, fw_crc);
nbytes += n;
}
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
}
printf("CRCs match\n");
exit(0);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||

12
src/drivers/rgbled/rgbled.cpp
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>");

8
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -404,10 +404,18 @@ void TECS::_update_pitch(void)
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;

65
src/modules/commander/commander.cpp
View File

@ -107,14 +107,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));
@ -890,14 +882,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);
}
}
@ -950,46 +940,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, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
status_changed = true;
}
/* End battery voltage check */

44
src/modules/commander/commander_helper.cpp
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;

7
src/modules/commander/commander_helper.h
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_ */

7
src/modules/commander/commander_params.c
View File

@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
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);

67
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
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);

4
src/modules/mavlink/mavlink.c
View File

@ -677,8 +677,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,

6
src/modules/px4iofirmware/dsm.c
View File

@ -203,6 +203,12 @@ dsm_guess_format(bool reset)
int
dsm_init(const char *device)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);

21
src/modules/px4iofirmware/px4io.c
View File

@ -125,6 +125,25 @@ heartbeat_blink(void)
LED_BLUE(heartbeat = !heartbeat);
}
static uint64_t reboot_time;
/**
schedule a reboot in time_delta_usec microseconds
*/
void schedule_reboot(uint32_t time_delta_usec)
{
reboot_time = hrt_absolute_time() + time_delta_usec;
}
/**
check for a scheduled reboot
*/
static void check_reboot(void)
{
if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
up_systemreset();
}
}
static void
calculate_fw_crc(void)
@ -249,6 +268,8 @@ user_start(int argc, char *argv[])
heartbeat_blink();
}
check_reboot();
#if 0
/* check for debug activity */
show_debug_messages();

4
src/modules/px4iofirmware/px4io.h
View File

@ -220,3 +220,7 @@ extern volatile uint8_t debug_level;
/** send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
/** schedule a reboot */
extern void schedule_reboot(uint32_t time_delta_usec);

13
src/modules/px4iofirmware/registers.c
View File

@ -518,16 +518,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
// check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC)
break;
// note that we don't set BL_WAIT_MAGIC in
// BKP_DR1 as that is not necessary given the
// timing of the forceupdate command. The
// bootloader on px4io waits for enough time
// anyway, and this method works with older
// bootloader versions (tested with both
// revision 3 and revision 4).
up_systemreset();
// we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK
// the reboot command
schedule_reboot(100000);
break;
case PX4IO_P_SETUP_DSM:

30
src/modules/sdlog2/sdlog2.c
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>
*
@ -702,6 +702,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));
@ -726,6 +727,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 */
@ -752,6 +754,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)
@ -760,9 +763,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 = 19;
/* 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];
@ -881,6 +884,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.
@ -971,8 +980,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_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;
@ -1238,6 +1245,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 */

14
src/modules/sdlog2/sdlog2_messages.h
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;
@ -248,6 +246,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 {
@ -281,7 +288,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"),
@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
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"),

2
src/modules/sensors/module.mk
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

187
src/modules/sensors/sensor_params.c
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);

89
src/modules/sensors/sensors.cpp
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);

60
src/modules/systemlib/board_serial.c Normal file
View File

@ -0,0 +1,60 @@
/****************************************************************************
*
* 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 board_serial.h
* Read off the board serial
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#include "otp.h"
#include "board_config.h"
#include "board_serial.h"
int get_board_serial(char *serialid)
{
const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
union udid id;
val_read((unsigned *)&id, udid_ptr, sizeof(id));
/* Copy the serial from the chips non-write memory and swap endianess */
serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0];
serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4];
serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8];
return 0;
}

49
src/modules/systemlib/board_serial.h Normal file
View File

@ -0,0 +1,49 @@
/****************************************************************************
*
* 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 board_serial.h
* Read off the board serial
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#pragma once
__BEGIN_DECLS
__EXPORT int get_board_serial(char *serialid);
__END_DECLS

3
src/modules/systemlib/bson/tinybson.c
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;
}

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

@ -49,4 +49,7 @@ SRCS = err.c \
airspeed.c \
system_params.c \
mavlink_log.c \
rc_check.c
rc_check.c \
otp.c \
board_serial.c

224
src/modules/systemlib/otp.c Normal file
View File

@ -0,0 +1,224 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Authors:
* Lorenz Meier <lm@inf.ethz.ch>
* David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
* 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 otp.c
* otp estimation
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#include <nuttx/config.h>
#include <board_config.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <string.h> // memset
#include "conversions.h"
#include "otp.h"
#include "err.h" // warnx
#include <assert.h>
int val_read(void *dest, volatile const void *src, int bytes)
{
int i;
for (i = 0; i < bytes / 4; i++) {
*(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i);
}
return i * 4;
}
int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature)
{
warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid);
int errors = 0;
// descriptor
if (F_write_byte(ADDR_OTP_START, 'P'))
errors++;
// write the 'P' from PX4. to first byte in OTP
if (F_write_byte(ADDR_OTP_START + 1, 'X'))
errors++; // write the 'P' from PX4. to first byte in OTP
if (F_write_byte(ADDR_OTP_START + 2, '4'))
errors++;
if (F_write_byte(ADDR_OTP_START + 3, '\0'))
errors++;
//id_type
if (F_write_byte(ADDR_OTP_START + 4, id_type))
errors++;
// vid and pid are 4 bytes each
if (F_write_word(ADDR_OTP_START + 5, vid))
errors++;
if (F_write_word(ADDR_OTP_START + 9, pid))
errors++;
// leave some 19 bytes of space, and go to the next block...
// then the auth sig starts
for (int i = 0 ; i < 128 ; i++) {
if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i]))
errors++;
}
return errors;
}
int lock_otp(void)
{
//determine the required locking size - can only write full lock bytes */
// int size = sizeof(struct otp) / 32;
//
// struct otp_lock otp_lock_mem;
//
// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem));
// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++)
// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED;
//XXX add the actual call here to write the OTP_LOCK bytes only at final stage
// val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem));
int locksize = 5;
int errors = 0;
// or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes.
for (int i = 0 ; i < locksize ; i++) {
if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED))
errors++;
}
return errors;
}
// COMPLETE, BUSY, or other flash error?
int F_GetStatus(void)
{
int fs = F_COMPLETE;
if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else {
if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else {
if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else {
if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else {
fs = F_COMPLETE;
}
}
}
}
return fs;
}
// enable FLASH Registers
void F_unlock(void)
{
if ((FLASH->control & F_CR_LOCK) != 0) {
FLASH->key = F_KEY1;
FLASH->key = F_KEY2;
}
}
// lock the FLASH Registers
void F_lock(void)
{
FLASH->control |= F_CR_LOCK;
}
// flash write word.
int F_write_word(uint32_t Address, uint32_t Data)
{
unsigned char octet[4] = {0, 0, 0, 0};
int ret = 0;
for (int i = 0; i < 4; i++) {
octet[i] = (Data >> (i * 8)) & 0xFF;
ret = F_write_byte(Address + i, octet[i]);
}
return ret;
}
// flash write byte
int F_write_byte(uint32_t Address, uint8_t Data)
{
volatile int status = F_COMPLETE;
//warnx("F_write_byte: %08X %02d", Address , Data ) ;
//Check the parameters
assert(IS_F_ADDRESS(Address));
//Wait for FLASH operation to complete by polling on BUSY flag.
status = F_GetStatus();
while (status == F_BUSY) { status = F_GetStatus();}
if (status == F_COMPLETE) {
//if the previous operation is completed, proceed to program the new data
FLASH->control &= CR_PSIZE_MASK;
FLASH->control |= F_PSIZE_BYTE;
FLASH->control |= F_CR_PG;
*(volatile uint8_t *)Address = Data;
//Wait for FLASH operation to complete by polling on BUSY flag.
status = F_GetStatus();
while (status == F_BUSY) { status = F_GetStatus();}
//if the program operation is completed, disable the PG Bit
FLASH->control &= (~F_CR_PG);
}
//Return the Program Status
return !(status == F_COMPLETE);
}

151
src/modules/systemlib/otp.h Normal file
View File

@ -0,0 +1,151 @@
/****************************************************************************
*
* 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 otp.h
* One TIme Programmable ( OTP ) Flash routine/s.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
*
*/
#ifndef OTP_H_
#define OTP_H_
__BEGIN_DECLS
#define ADDR_OTP_START 0x1FFF7800
#define ADDR_OTP_LOCK_START 0x1FFF7A00
#define OTP_LOCK_LOCKED 0x00
#define OTP_LOCK_UNLOCKED 0xFF
#include <unistd.h>
#include <stdio.h>
// possible flash statuses
#define F_BUSY 1
#define F_ERROR_WRP 2
#define F_ERROR_PROGRAM 3
#define F_ERROR_OPERATION 4
#define F_COMPLETE 5
typedef struct {
volatile uint32_t accesscontrol; // 0x00
volatile uint32_t key; // 0x04
volatile uint32_t optionkey; // 0x08
volatile uint32_t status; // 0x0C
volatile uint32_t control; // 0x10
volatile uint32_t optioncontrol; //0x14
} flash_registers;
#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address
#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000)
#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00)
#define FLASH ((flash_registers *) F_R_BASE)
#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit
#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit
#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit
#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF)
#define F_PSIZE_WORD ((uint32_t)0x00000200)
#define F_PSIZE_BYTE ((uint32_t)0x00000000)
#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register
#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit.
#define F_KEY1 ((uint32_t)0x45670123)
#define F_KEY2 ((uint32_t)0xCDEF89AB)
#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F)))
#pragma pack(push, 1)
/*
* The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes.
* The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15)
* to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed
* until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only
* contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly.
*/
struct otp {
// first 32 bytes = the '0' Block
char id[4]; ///4 bytes < 'P' 'X' '4' '\n'
uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID
uint32_t vid; ///4 bytes
uint32_t pid; ///4 bytes
char unused[19]; ///19 bytes
// Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block )
char signature[128];
// insert extras here
uint32_t lock_bytes[4];
};
struct otp_lock {
uint8_t lock_bytes[16];
};
#pragma pack(pop)
#define ADDR_F_SIZE 0x1FFF7A22
#pragma pack(push, 1)
union udid {
uint32_t serial[3];
char data[12];
};
#pragma pack(pop)
/**
* s
*/
//__EXPORT float calc_indicated_airspeed(float differential_pressure);
__EXPORT void F_unlock(void);
__EXPORT void F_lock(void);
__EXPORT int val_read(void *dest, volatile const void *src, int bytes);
__EXPORT int val_write(volatile void *dest, const void *src, int bytes);
__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature);
__EXPORT int lock_otp(void);
__EXPORT int F_write_byte(uint32_t Address, uint8_t Data);
__EXPORT int F_write_word(uint32_t Address, uint32_t Data);
__END_DECLS
#endif

35
src/modules/systemlib/param/param.c
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());

9
src/modules/uORB/topics/battery_status.h
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

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

@ -1,265 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file eeprom.c
*
* EEPROM service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <board_config.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
#ifndef PX4_I2C_BUS_ONBOARD
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
#endif
__EXPORT int eeprom_main(int argc, char *argv[]);
static void eeprom_attach(void);
static void eeprom_start(void);
static void eeprom_erase(void);
static void eeprom_ioctl(unsigned operation);
static void eeprom_save(const char *name);
static void eeprom_load(const char *name);
static void eeprom_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *eeprom_mtd;
int eeprom_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
eeprom_start();
if (!strcmp(argv[1], "save_param"))
eeprom_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
eeprom_load(argv[2]);
if (!strcmp(argv[1], "erase"))
eeprom_erase();
if (!strcmp(argv[1], "test"))
eeprom_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
eeprom_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
eeprom_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
}
static void
eeprom_attach(void)
{
/* find the right I2C */
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
/* this resets the I2C bus, set correct bus speed again */
I2C_SETFREQUENCY(i2c, 400000);
if (i2c == NULL)
errx(1, "failed to locate I2C bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
eeprom_mtd = at24c_initialize(i2c);
if (eeprom_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: EEPROM needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (eeprom_mtd == NULL)
errx(1, "failed to initialize EEPROM driver");
attached = true;
}
static void
eeprom_start(void)
{
int ret;
if (started)
errx(1, "EEPROM already mounted");
if (!attached)
eeprom_attach();
/* start NXFFS */
ret = nxffs_initialize(eeprom_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
/* mount the EEPROM */
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
started = true;
warnx("mounted EEPROM at /eeprom");
exit(0);
}
extern int at24c_nuke(void);
static void
eeprom_erase(void)
{
if (!attached)
eeprom_attach();
if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
eeprom_ioctl(unsigned operation)
{
int fd;
fd = open("/eeprom/.", 0);
if (fd < 0)
err(1, "open /eeprom");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
eeprom_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
eeprom_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
extern void at24c_test(void);
static void
eeprom_test(void)
{
at24c_test();
exit(0);
}

1
src/systemcmds/esc_calib/esc_calib.c
View File

@ -148,6 +148,7 @@ esc_calib_main(int argc, char *argv[])
case 'l':
/* Read in custom low value */
pwm_low = strtoul(optarg, &ep, 0);
if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN)
usage("low PWM invalid");
break;

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

6
src/systemcmds/mtd/module.mk Normal file
View File

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

378
src/systemcmds/mtd/mtd.c Normal file
View File

@ -0,0 +1,378 @@
/****************************************************************************
*
* 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_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], "status"))
mtd_status();
if (!strcmp(argv[1], "erase")) {
if (argc < 3) {
errx(1, "usage: mtd erase <PARTITION_PATH..>");
}
mtd_erase(argv + 2, argc - 2);
}
}
errx(1, "expected a command, try 'start', 'erase' 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, 40 * 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");
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;
}
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);
}
#endif

4
src/systemcmds/nshterm/nshterm.c
View File

@ -62,7 +62,9 @@ nshterm_main(int argc, char *argv[])
}
uint8_t retries = 0;
int fd = -1;
while (retries < 50) {
/* try the first 30 seconds */
while (retries < 300) {
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);

47
src/systemcmds/param/param.c
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: ",

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

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

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

@ -1,279 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ramtron.c
*
* ramtron service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/spi.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int ramtron_main(int argc, char *argv[]);
#ifndef CONFIG_MTD_RAMTRON
/* create a fake command with decent message to not confuse users */
int ramtron_main(int argc, char *argv[])
{
errx(1, "RAMTRON not enabled, skipping.");
}
#else
static void ramtron_attach(void);
static void ramtron_start(void);
static void ramtron_erase(void);
static void ramtron_ioctl(unsigned operation);
static void ramtron_save(const char *name);
static void ramtron_load(const char *name);
static void ramtron_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *ramtron_mtd;
int ramtron_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
ramtron_start();
if (!strcmp(argv[1], "save_param"))
ramtron_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
ramtron_load(argv[2]);
if (!strcmp(argv[1], "erase"))
ramtron_erase();
if (!strcmp(argv[1], "test"))
ramtron_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
ramtron_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
ramtron_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
}
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
static void
ramtron_attach(void)
{
/* find the right spi */
struct spi_dev_s *spi = up_spiinitialize(2);
/* this resets the spi bus, set correct bus speed again */
// xxx set in ramtron driver, leave this out
// SPI_SETFREQUENCY(spi, 4000000);
SPI_SETFREQUENCY(spi, 375000000);
SPI_SETBITS(spi, 8);
SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false);
if (spi == NULL)
errx(1, "failed to locate spi bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
ramtron_mtd = ramtron_initialize(spi);
if (ramtron_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: ramtron needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (ramtron_mtd == NULL)
errx(1, "failed to initialize ramtron driver");
attached = true;
}
static void
ramtron_start(void)
{
int ret;
if (started)
errx(1, "ramtron already mounted");
if (!attached)
ramtron_attach();
/* start NXFFS */
ret = nxffs_initialize(ramtron_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
/* mount the ramtron */
ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /ramtron - erase ramtron to reformat");
started = true;
warnx("mounted ramtron at /ramtron");
exit(0);
}
//extern int at24c_nuke(void);
static void
ramtron_erase(void)
{
if (!attached)
ramtron_attach();
// if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
ramtron_ioctl(unsigned operation)
{
int fd;
fd = open("/ramtron/.", 0);
if (fd < 0)
err(1, "open /ramtron");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
ramtron_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
ramtron_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
//extern void at24c_test(void);
static void
ramtron_test(void)
{
// at24c_test();
exit(0);
}
#endif

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

@ -29,4 +29,5 @@ SRCS = test_adc.c \
test_param.c \
test_ppm_loopback.c \
test_rc.c \
test_conv.cpp
test_conv.cpp \
test_mount.c

163
src/systemcmds/tests/test_file.c
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
@ -35,9 +35,12 @@
* @file test_file.c
*
* File write test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <poll.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
@ -51,6 +54,40 @@
#include "tests.h"
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;
}
int
test_file(int argc, char *argv[])
{
@ -86,7 +123,6 @@ test_file(int argc, char *argv[])
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
hrt_abstime start, end;
//perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
@ -94,28 +130,25 @@ test_file(int argc, char *argv[])
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
//perf_begin(wperf);
int wret = write(fd, write_buf + a, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
warn("WRITE ERROR!");
if ((0x3 & (uintptr_t)(write_buf + a)))
errx(1, "memory is unaligned, align shift: %d", a);
warnx("memory is unaligned, align shift: %d", a);
return 1;
}
fsync(fd);
//perf_end(wperf);
if (!check_user_abort(fd))
return OK;
}
end = hrt_absolute_time();
//warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
//perf_print_counter(wperf);
//perf_free(wperf);
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
@ -124,7 +157,8 @@ test_file(int argc, char *argv[])
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
errx(1, "READ ERROR!");
warnx("READ ERROR!");
return 1;
}
/* compare value */
@ -139,9 +173,13 @@ test_file(int argc, char *argv[])
}
if (!compare_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
return 1;
}
if (!check_user_abort(fd))
return OK;
}
/*
@ -152,16 +190,20 @@ test_file(int argc, char *argv[])
int ret = unlink("/fs/microsd/testfile");
fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
warnx("testing aligned writes - please wait..");
warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
err(1, "WRITE ERROR!");
warnx("WRITE ERROR!");
return 1;
}
if (!check_user_abort(fd))
return OK;
}
fsync(fd);
@ -178,7 +220,8 @@ test_file(int argc, char *argv[])
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
err(1, "READ ERROR!");
warnx("READ ERROR!");
return 1;
}
/* compare value */
@ -190,10 +233,14 @@ test_file(int argc, char *argv[])
align_read_ok = false;
break;
}
if (!check_user_abort(fd))
return OK;
}
if (!align_read_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
return 1;
}
}
@ -215,7 +262,8 @@ test_file(int argc, char *argv[])
int rret = read(fd, read_buf + a, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
err(1, "READ ERROR!");
warnx("READ ERROR!");
return 1;
}
for (int j = 0; j < chunk_sizes[c]; j++) {
@ -228,10 +276,14 @@ test_file(int argc, char *argv[])
if (unalign_read_err_count > 10)
break;
}
if (!check_user_abort(fd))
return OK;
}
if (!unalign_read_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
return 1;
}
}
@ -239,9 +291,10 @@ test_file(int argc, char *argv[])
ret = unlink("/fs/microsd/testfile");
close(fd);
if (ret)
err(1, "UNLINKING FILE FAILED");
if (ret) {
warnx("UNLINKING FILE FAILED");
return 1;
}
}
}
@ -261,75 +314,9 @@ test_file(int argc, char *argv[])
} else {
/* failed opening dir */
err(1, "FAILED LISTING MICROSD ROOT DIRECTORY");
}
return 0;
}
#if 0
int
test_file(int argc, char *argv[])
{
const iterations = 1024;
/* check if microSD card is mounted */
struct stat buffer;
if (stat("/fs/microsd/", &buffer)) {
warnx("no microSD card mounted, aborting file test");
warnx("FAILED LISTING MICROSD ROOT DIRECTORY");
return 1;
}
uint8_t buf[512];
hrt_abstime start, end;
perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
memset(buf, 0, sizeof(buf));
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
perf_begin(wperf);
write(fd, buf, sizeof(buf));
perf_end(wperf);
}
end = hrt_absolute_time();
close(fd);
unlink("/fs/microsd/testfile");
warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
perf_print_counter(wperf);
perf_free(wperf);
warnx("running unlink test");
/* ensure that common commands do not run against file count limits */
for (unsigned i = 0; i < 64; i++) {
warnx("unlink iteration #%u", i);
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
if (fd < 0)
errx(1, "failed opening test file before unlink()");
int ret = write(fd, buf, sizeof(buf));
if (ret < 0)
errx(1, "failed writing test file before unlink()");
close(fd);
ret = unlink("/fs/microsd/testfile");
if (ret != OK)
errx(1, "failed unlinking test file");
fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
if (fd < 0)
errx(1, "failed opening test file after unlink()");
ret = write(fd, buf, sizeof(buf));
if (ret < 0)
errx(1, "failed writing test file after unlink()");
close(fd);
}
return 0;
}
#endif

289
src/systemcmds/tests/test_mount.c Normal file
View File

@ -0,0 +1,289 @@
/****************************************************************************
*
* 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_mount.c
*
* Device mount / unmount stress test
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
const int fsync_tries = 1;
const int abort_tries = 10;
int
test_mount(int argc, char *argv[])
{
const unsigned iterations = 2000;
const unsigned alignments = 10;
const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt";
/* check if microSD card is mounted */
struct stat buffer;
if (stat("/fs/microsd/", &buffer)) {
warnx("no microSD card mounted, aborting file test");
return 1;
}
/* list directory */
DIR *d;
struct dirent *dir;
d = opendir("/fs/microsd");
if (d) {
while ((dir = readdir(d)) != NULL) {
//printf("%s\n", dir->d_name);
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
warnx("FAILED LISTING MICROSD ROOT DIRECTORY");
if (stat(cmd_filename, &buffer) == OK) {
(void)unlink(cmd_filename);
}
return 1;
}
/* read current test status from file, write test instructions for next round */
/* initial values */
int it_left_fsync = fsync_tries;
int it_left_abort = abort_tries;
int cmd_fd;
if (stat(cmd_filename, &buffer) == OK) {
/* command file exists, read off state */
cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK);
char buf[64];
int ret = read(cmd_fd, buf, sizeof(buf));
if (ret > 0) {
int count = 0;
ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count);
} else {
buf[0] = '\0';
}
if (it_left_fsync > fsync_tries)
it_left_fsync = fsync_tries;
if (it_left_abort > abort_tries)
it_left_abort = abort_tries;
warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort,
fsync_tries, abort_tries, buf);
int it_left_fsync_prev = it_left_fsync;
/* now write again what to do next */
if (it_left_fsync > 0)
it_left_fsync--;
if (it_left_fsync == 0 && it_left_abort > 0) {
it_left_abort--;
/* announce mode switch */
if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
fsync(stdout);
fsync(stderr);
usleep(20000);
}
}
if (it_left_abort == 0) {
(void)unlink(cmd_filename);
return 0;
}
} else {
/* this must be the first iteration, do something */
cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT);
warnx("First iteration of file test\n");
}
char buf[64];
int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
lseek(cmd_fd, 0, SEEK_SET);
write(cmd_fd, buf, strlen(buf) + 1);
fsync(cmd_fd);
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
fsync(stdout);
fsync(stderr);
usleep(50000);
for (unsigned a = 0; a < alignments; a++) {
printf(".");
uint8_t write_buf[chunk_sizes[c] + alignments] __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] + alignments] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf + a, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
warn("WRITE ERROR!");
if ((0x3 & (uintptr_t)(write_buf + a)))
warnx("memory is unaligned, align shift: %d", a);
return 1;
}
if (it_left_fsync > 0) {
fsync(fd);
} else {
printf("#");
fsync(stdout);
fsync(stderr);
}
}
if (it_left_fsync > 0) {
printf("#");
}
printf(".");
fsync(stdout);
fsync(stderr);
usleep(200000);
end = hrt_absolute_time();
close(fd);
fd = open("/fs/microsd/testfile", 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!");
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j + a]) {
warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
compare_ok = false;
break;
}
}
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
return 1;
}
}
int ret = unlink("/fs/microsd/testfile");
close(fd);
if (ret) {
warnx("UNLINKING FILE FAILED");
return 1;
}
}
}
fsync(stdout);
fsync(stderr);
usleep(20000);
/* we always reboot for the next test if we get here */
warnx("Iteration done, rebooting..");
fsync(stdout);
fsync(stderr);
usleep(50000);
systemreset(false);
/* never going to get here */
return 0;
}

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

@ -110,6 +110,7 @@ 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[]);
__END_DECLS

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

@ -107,6 +107,7 @@ const struct {
{"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},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}
};