Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

apps/commander/commander.c

@@ -61,7 +61,7 @@
 #include <sys/prctl.h>
 #include <v1.0/common/mavlink.h>
 #include <string.h>
-#include <arch/board/drv_led.h>
+#include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
@@ -194,7 +194,7 @@ static void buzzer_deinit()
 
 static int led_init()
 {
-	leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
+	leds = open(LED_DEVICE_PATH, 0);
 
 	if (leds < 0) {
 		fprintf(stderr, "[commander] LED: open fail\n");
@@ -268,33 +268,6 @@ void tune_confirm() {
 	ioctl(buzzer, TONE_SET_ALARM, 3);
 }
 
-static const char *parameter_file = "/eeprom/parameters";
-
-static int pm_save_eeprom(bool only_unsaved)
-{
-	/* delete the file in case it exists */
-	unlink(parameter_file);
-
-	/* create the file */
-	int fd = open(parameter_file, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0) {
-		warn("opening '%s' for writing failed", parameter_file);
-		return -1;
-	}
-
-	int result = param_export(fd, only_unsaved);
-	close(fd);
-
-	if (result != 0) {
-		unlink(parameter_file);
-		warn("error exporting parameters to '%s'", parameter_file);
-		return -2;
-	}
-
-	return 0;
-}
-
 void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	/* set to mag calibration mode */
@@ -363,6 +336,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	if (x == NULL || y == NULL || z == NULL) {
 		warnx("mag cal failed: out of memory");
 		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
+		printf("x:%p y:%p z:%p\n", x, y, z);
 		return;
 	}
 
@@ -495,9 +469,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 
 		/* auto-save to EEPROM */
-		int save_ret = pm_save_eeprom(false);
+		int save_ret = param_save_default();
 		if(save_ret != 0) {
-			warn("WARNING: auto-save of params to EEPROM failed");
+			warn("WARNING: auto-save of params to storage failed");
 		}
 
 		printf("[mag cal]\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -615,9 +589,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 		close(fd);
 
 		/* auto-save to EEPROM */
-		int save_ret = pm_save_eeprom(false);
+		int save_ret = param_save_default();
 		if(save_ret != 0) {
-			warn("WARNING: auto-save of params to EEPROM failed");
+			warn("WARNING: auto-save of params to storage failed");
 		}
 
 		// char buf[50];
@@ -735,9 +709,9 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 		close(fd);
 
 		/* auto-save to EEPROM */
-		int save_ret = pm_save_eeprom(false);
+		int save_ret = param_save_default();
 		if(save_ret != 0) {
-			warn("WARNING: auto-save of params to EEPROM failed");
+			warn("WARNING: auto-save of params to storage failed");
 		}
 
 		//char buf[50];
@@ -1101,7 +1075,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
-					 9000,
+					 4096,
 					 commander_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;

apps/drivers/boards/px4fmu/px4fmu_init.c

@@ -64,10 +64,9 @@
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
-#include <arch/board/drv_led.h>
-#include <arch/board/drv_eeprom.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
 
 #include <systemlib/cpuload.h>
 
@@ -132,9 +131,6 @@ __EXPORT void stm32_boardinitialize(void)
 
 static struct spi_dev_s *spi1;
 static struct spi_dev_s *spi3;
-static struct i2c_dev_s *i2c1;
-static struct i2c_dev_s *i2c2;
-static struct i2c_dev_s *i2c3;
 
 #include <math.h>
 
@@ -154,10 +150,6 @@ __EXPORT int nsh_archinitialize(void)
 {
 	int result;
 
-	/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
-
-	/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
-
 	/* configure the high-resolution time/callout interface */
 #ifdef CONFIG_HRT_TIMER
 	hrt_init();
@@ -191,14 +183,12 @@ __EXPORT int nsh_archinitialize(void)
 
 	message("\r\n");
 
+	// initial LED state
+	drv_led_start();
 	up_ledoff(LED_BLUE);
 	up_ledoff(LED_AMBER);
-
 	up_ledon(LED_BLUE);
 
-	/* Configure user-space led driver */
-	px4fmu_led_init();
-
 	/* Configure SPI-based devices */
 
 	spi1 = up_spiinitialize(1);
@@ -220,37 +210,6 @@ __EXPORT int nsh_archinitialize(void)
 
 	message("[boot] Successfully initialized SPI port 1\r\n");
 
-	/* initialize I2C2 bus */
-
-	i2c2 = up_i2cinitialize(2);
-
-	if (!i2c2) {
-		message("[boot] FAILED to initialize I2C bus 2\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* set I2C2 speed */
-	I2C_SETFREQUENCY(i2c2, 400000);
-
-
-	i2c3 = up_i2cinitialize(3);
-
-	if (!i2c3) {
-		message("[boot] FAILED to initialize I2C bus 3\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* set I2C3 speed */
-	I2C_SETFREQUENCY(i2c3, 400000);
-
-	/* try to attach, don't fail if device is not responding */
-	(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
-			    FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
-			    FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
-			    FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
-
 #if defined(CONFIG_STM32_SPI3)
 	/* Get the SPI port */
 
@@ -277,23 +236,6 @@ __EXPORT int nsh_archinitialize(void)
 	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
 #endif /* SPI3 */
 
-	/* initialize I2C1 bus */
-
-	i2c1 = up_i2cinitialize(1);
-
-	if (!i2c1) {
-		message("[boot] FAILED to initialize I2C bus 1\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* set I2C1 speed */
-	I2C_SETFREQUENCY(i2c1, 400000);
-
-	/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
-
-	/* Get board information if available */
-
 #ifdef CONFIG_ADC
 	int adc_state = adc_devinit();
 

apps/drivers/boards/px4fmu/px4fmu_led.c

@@ -31,10 +31,10 @@
  *
  ****************************************************************************/
 
-/*
+/**
- * led driver for PX4FMU
+ * @file px4fmu_led.c
  *
- * This is something of an experiment currently (ha, get it?)
+ * PX4FMU LED backend.
  */
 
 #include <nuttx/config.h>
@@ -43,71 +43,46 @@
 #include <stdbool.h>
 #include <debug.h>
 
-#include <nuttx/spi.h>
 #include <arch/board/board.h>
 
-#include "up_arch.h"
 #include "chip.h"
+#include "up_arch.h"
+#include "up_internal.h"
 #include "stm32_internal.h"
-#include "px4fmu-internal.h"
+#include "px4fmu_internal.h"
 
-#include <arch/board/drv_led.h>
+__EXPORT void up_ledinit()
-
-static int	px4fmu_led_ioctl(struct file *filep, int cmd, unsigned long arg);
-static ssize_t	px4fmu_led_pseudoread(struct file *filp, FAR char *buffer, size_t buflen);
-
-static const struct file_operations px4fmu_led_fops = {
-	.read  = px4fmu_led_pseudoread,
-	.ioctl = px4fmu_led_ioctl,
-};
-
-int
-px4fmu_led_init(void)
 {
-	/* register the driver */
+	/* Configure LED1-2 GPIOs for output */
-	return register_driver("/dev/led", &px4fmu_led_fops, 0666, NULL);
+
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
 }
 
-static ssize_t
+__EXPORT void up_ledon(int led)
-px4fmu_led_pseudoread(struct file *filp, FAR char *buffer, size_t buflen)
 {
-	return 0;
+	if (led == 0)
-}
+	{
-
+		/* Pull down to switch on */
-static int
+		stm32_gpiowrite(GPIO_LED1, false);
-px4fmu_led_ioctl(struct file *filep, int cmd, unsigned long arg)
+	}
-{
+	if (led == 1)
-	int	result = 0;
+	{
-
+		/* Pull down to switch on */
-	switch (cmd) {
+		stm32_gpiowrite(GPIO_LED2, false);
-
-	case LED_ON:
-		switch (arg) {
-		case 0:
-		case 1:
-			up_ledon(arg);
-			break;
-		default:
-			result = -1;
-			break;
-		}
-		break;
-
-	case LED_OFF:
-		switch (arg) {
-		case 0:
-		case 1:
-			up_ledoff(arg);
-			break;
-		default:
-			result = -1;
-			break;
-		}
-		break;
-		default:
-			result = -1;
-			break;
 	}
-	return result;
 }
 
+__EXPORT void up_ledoff(int led)
+{
+	if (led == 0)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED2, true);
+	}
+}

apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c

@@ -46,13 +46,9 @@
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
 
-#include "chip.h"
+#include <stm32_internal.h>
-#include "up_internal.h"
+#include <stm32_gpio.h>
-#include "up_arch.h"
+#include <stm32_tim.h>
-
-#include "stm32_internal.h"
-#include "stm32_gpio.h"
-#include "stm32_tim.h"
 
 __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
 	{

apps/drivers/drv_led.h

@@ -31,21 +31,34 @@
  *
  ****************************************************************************/
 
+/**
+ * @file drv_led.h
+ *
+ * LED driver API
+ */
+
+#pragma once
+
+#include <stdint.h>
 #include <sys/ioctl.h>
 
-#define _LEDCBASE	0x6800
+#define LED_DEVICE_PATH		"/dev/led"
-#define LEDC(_x)	_IOC(_LEDCBASE, _x)
 
-/* set the LED identified by the argument */
+#define _LED_BASE		0x2800
-#define LED_ON	LEDC(1)
 
-/* clear the LED identified by the argument */
+/* PX4 LED colour codes */
-#define LED_OFF	LEDC(2)
+#define LED_AMBER		0
+#define LED_RED			0	/* some boards have red rather than amber */
+#define LED_BLUE		1
 
-///* toggle the LED identified by the argument */
+#define LED_ON			_IOC(_LED_BASE, 0)
-//#define LED_TOGGLE	LEDC(3)
+#define LED_OFF			_IOC(_LED_BASE, 1)
 
-#define LED_BLUE		  0  /* Led on first port */
+__BEGIN_DECLS
-#define LED_AMBER		  1  /* Led on second port */
 
-extern int	px4fmu_led_init(void);
+/*
+ * Initialise the LED driver.
+ */
+__EXPORT extern void drv_led_start();
+
+__END_DECLS

apps/drivers/led/Makefile

@@ -32,11 +32,7 @@
 ############################################################################
 
 #
-# Makefile to build ardrone interface
+# Makefile to build the LED driver.
 #
 
-APPNAME		 = led
-PRIORITY	 = SCHED_PRIORITY_MAX - 15
-STACKSIZE	 = 2048
-
 include $(APPDIR)/mk/app.mk

apps/drivers/led/led.cpp

@@ -32,29 +32,84 @@
  ****************************************************************************/
 
 /**
- * @file drv_eeprom.h
+ * @file led.cpp
  *
- * Config for the non-MTD EEPROM driver.
+ * LED driver.
  */
 
-/* IMPORTANT: Adjust this number! */
+#include <nuttx/config.h>
-#define MAX_EEPROMS					2
+#include <drivers/device/device.h>
+#include <drivers/drv_led.h>
 
-/* FMU onboard */
+/* Ideally we'd be able to get these from up_internal.h */
-#define FMU_BASEBOARD_EEPROM_ADDRESS			0x57
+//#include <up_internal.h>
-#define FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES		128
+__BEGIN_DECLS
-#define FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES		8
+extern void up_ledinit();
-#define FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US		3300
+extern void up_ledon(int led);
-#define FMU_BASEBOARD_EEPROM_BUS_CLOCK			400000    ///< 400 KHz max. clock
+extern void up_ledoff(int led);
+__END_DECLS
 
-/**
+class LED : device::CDev
- * @brief i2c I2C bus struct
+{
- * @brief device_address The device address as stated in the datasheet, e.g. for a Microchip 24XX128 0x50 with all ID pins tied to GND
+public:
- * @brief total_size_bytes The total size in bytes, e.g. 16K = 16000 bytes for the Microchip 24XX128
+	LED();
- * @brief page_size_bytes The size of one page, e.g. 64 bytes for the Microchip 24XX128
+	~LED();
- * @brief device_name The device name to register this device to, e.g. /dev/eeprom
- * @brief fail_if_missing Returns error if the EEPROM was not found. This is helpful if the EEPROM might be attached later when the board is running
- */
-extern int
-eeprom_attach(struct i2c_dev_s *i2c, uint8_t device_address, uint16_t total_size_bytes, uint16_t page_size_bytes, uint16_t page_write_time_us, const char* device_name, uint8_t fail_if_missing);
 
+	virtual int		init();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+};
+
+LED::LED() :
+	CDev("led", LED_DEVICE_PATH)
+{
+	// force immediate init/device registration
+	init();
+}
+
+LED::~LED()
+{
+}
+
+int
+LED::init()
+{
+	CDev::init();
+	up_ledinit();
+
+	return 0;
+}
+
+int
+LED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	switch (cmd) {
+	case LED_ON:
+		up_ledon(arg);
+		break;
+
+	case LED_OFF:
+		up_ledoff(arg);
+		break;
+
+	default:
+		result = CDev::ioctl(filp, cmd, arg);
+	}
+	return result;
+}
+
+namespace
+{
+LED	*gLED;
+}
+
+void
+drv_led_start()
+{
+	if (gLED == nullptr) {
+		gLED = new LED;
+		if (gLED != nullptr)
+			gLED->init();
+	}
+}

apps/gps/ubx.c

@@ -479,6 +479,10 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 					if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
 
 						ubx_gps->vel = (uint16_t)packet->speed;
+						ubx_gps->vel_n = packet->velN / 100.0f;
+						ubx_gps->vel_e = packet->velE / 100.0f;
+						ubx_gps->vel_d = packet->velD / 100.0f;
+						ubx_gps->vel_ned_valid = true;
 						ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
 
 						ubx_gps->timestamp = hrt_absolute_time();

apps/mavlink/mavlink_parameters.c

@@ -172,64 +172,6 @@ int mavlink_pm_send_param(param_t param)
 	return ret;
 }
 
-static const char *mavlink_parameter_file = "/eeprom/parameters";
-
-/**
- * @return 0 on success, -1 if device open failed, -2 if writing parameters failed
- */
-static int mavlink_pm_save_eeprom()
-{
-	/* delete the file in case it exists */
-	unlink(mavlink_parameter_file);
-
-	/* create the file */
-	int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0) {
-		warn("opening '%s' for writing failed", mavlink_parameter_file);
-		return -1;
-	}
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result != 0) {
-		unlink(mavlink_parameter_file);
-		warn("error exporting parameters to '%s'", mavlink_parameter_file);
-		return -2;
-	}
-
-	return 0;
-}
-
-/**
- * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
- */
-static int
-mavlink_pm_load_eeprom()
-{
-	int fd = open(mavlink_parameter_file, O_RDONLY);
-
-	if (fd < 0) {
-		/* no parameter file is OK, otherwise this is an error */
-		if (errno != ENOENT) {
-			warn("open '%s' for reading failed", mavlink_parameter_file);
-			return -1;
-		}
-		return 1;
-	}
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result != 0) {
-		warn("error reading parameters from '%s'", mavlink_parameter_file);
-		return -2;
-	}
-
-	return 0;
-}
-
 void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
 {
 	switch (msg->msgid) {
@@ -307,7 +249,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 					if (((int)(cmd_mavlink.param1)) == 0)	{
 
 						/* read all parameters from EEPROM to RAM */
-						int read_ret = mavlink_pm_load_eeprom();
+						int read_ret = param_load_default();
 						if (read_ret == OK) {
 							//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
 							mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
@@ -327,7 +269,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 					} else if (((int)(cmd_mavlink.param1)) == 1)	{
 
 						/* write all parameters from RAM to EEPROM */
-						int write_ret = mavlink_pm_save_eeprom();
+						int write_ret = param_save_default();
 						if (write_ret == OK) {
 							mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM");
 							result = MAV_RESULT_ACCEPTED;

apps/mavlink/mavlink_receiver.c

@@ -253,14 +253,13 @@ handle_message(mavlink_message_t *msg)
 					break;
 			}
 
-			offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid]   / (float)INT16_MAX;
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]  / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid]  / (float)INT16_MAX;
-			offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid]    / (float)INT16_MAX;
+			offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid]    / (float)INT16_MAX;
 			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid] == 0) {
 				ml_armed = false;
-
 			}
 
 			offboard_control_sp.armed = ml_armed;

apps/px4/tests/test_eeproms.c

@@ -1,328 +0,0 @@
-/****************************************************************************
- * px4/eeproms/test_eeproms.c
- *
- *  Copyright (C) 2012 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 NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#include "tests.h"
-
-#include <arch/board/drv_eeprom.h>
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-static int  onboard_eeprom(int argc, char *argv[]);
-static int	baseboard_eeprom(int argc, char *argv[]);
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-struct {
-	const char	*name;
-	const char	*path;
-	int	(* test)(int argc, char *argv[]);
-} eeproms[] = {
-	{"onboard_eeprom",	"/dev/eeprom",	onboard_eeprom},
-	{"baseboard_eeprom",	"/dev/baseboard_eeprom",	baseboard_eeprom},
-	{NULL, NULL, NULL}
-};
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-static int
-onboard_eeprom(int argc, char *argv[])
-{
-	printf("\tonboard_eeprom: test start\n");
-	fflush(stdout);
-
-	int		fd;
-	uint8_t	buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '};
-	int		ret;
-	bool force_write = false;
-	if (strcmp(argv[0], "jig") == 0) force_write = true;
-
-	/* fill with spaces */
-	//memset(buf1+16, 'x', sizeof(buf1-16));
-
-	/* fill in some magic values at magic positions */
-	buf1[63] = 'E';
-	buf1[64] = 'S';
-	buf1[127] = 'F';
-	buf1[128] = 'T';
-
-	/* terminate string */
-	buf1[sizeof(buf1) - 1] = '\0';
-
-	fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
-
-	if (fd < 0) {
-		printf("onboard eeprom: open fail\n");
-		return ERROR;
-	}
-
-	/* read data */
-	ret = read(fd, buf1, 1);
-
-	if (ret != 1) {
-		printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret);
-
-		switch (-ret) {
-		case EPERM:
-			printf("\treason: %s\n", EPERM_STR);
-			break;
-
-		case ENOENT:
-			printf("\treason: %s\n", ENOENT_STR);
-			break;
-
-		case ESRCH:
-			printf("\treason: %s\n", ESRCH_STR);
-			break;
-
-		case EINTR:
-			printf("\treason: %s\n", EINTR_STR);
-			break;
-
-		}
-	}
-
-	printf("\tonboard eeprom: first byte: %d\n", buf1[0]);
-	if (!force_write) {
-		printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n");
-
-		printf("Input: ");
-		char c = getchar();
-		printf("%c\n", c);
-		if (c != 'y' && c != 'Y') {
-			/* not yes, abort */
-			close(fd);
-
-			/* Let user know everything is ok */
-			printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n");
-			return OK;
-		}
-	}
-
-	printf("\tonboard eeprom: proceeding with write test\r\n");
-
-	/* increment counter */
-	buf1[0] = buf1[0] + 1;
-
-	/* rewind to the start of the file */
-	lseek(fd, 0, SEEK_SET);
-
-	/* write data */
-	ret = write(fd, buf1, sizeof(buf1));
-
-	if (ret != sizeof(buf1)) {
-		printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret);
-
-		switch (-ret) {
-		case EPERM:
-			printf("\treason: %s\n", EPERM_STR);
-			break;
-
-		case ENOENT:
-			printf("\treason: %s\n", ENOENT_STR);
-			break;
-
-		case ESRCH:
-			printf("\treason: %s\n", ESRCH_STR);
-			break;
-
-		case EINTR:
-			printf("\treason: %s\n", EINTR_STR);
-			break;
-
-		}
-
-		//return ERROR;
-	}
-
-	/* rewind to the start of the file */
-	lseek(fd, 0, SEEK_SET);
-
-	/* read data */
-	ret = read(fd, buf1, sizeof(buf1));
-
-	if (ret != sizeof(buf1)) {
-		printf("\tonboard eeprom: ERROR: read fail: %d\n", ret);
-
-		switch (-ret) {
-		case EPERM:
-			printf("\treason: %s\n", EPERM_STR);
-			break;
-
-		case ENOENT:
-			printf("\treason: %s\n", ENOENT_STR);
-			break;
-
-		case ESRCH:
-			printf("\treason: %s\n", ESRCH_STR);
-			break;
-
-		case EINTR:
-			printf("\treason: %s\n", EINTR_STR);
-			break;
-
-		}
-
-		return ERROR;
-
-	} else {
-		/* enforce null termination and print as string */
-		if (buf1[sizeof(buf1) - 1] != 0) {
-			printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n");
-			buf1[sizeof(buf1) - 1] = '\0';
-		}
-
-		/* read out counter and replace val */
-		int counter = buf1[0];
-		printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1);
-		printf("\tAll %d bytes:\n\n\t", sizeof(buf1));
-
-		for (int i = 0; i < sizeof(buf1); i++) {
-			printf("0x%02x ", buf1[i]);
-
-			if (i % 8 == 7) printf("\n\t");
-
-			if (i % 64 == 63) printf("\n\t");
-		}
-
-		/* end any open line */
-		printf("\n\n");
-	}
-
-	close(fd);
-
-	/* Let user know everything is ok */
-	printf("\tOK: onboard eeprom passed all tests successfully\n");
-	return ret;
-}
-
-static int
-baseboard_eeprom(int argc, char *argv[])
-{
-	printf("\tbaseboard eeprom: test start\n");
-	fflush(stdout);
-
-	int		fd;
-	uint8_t	buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'};
-	int		ret;
-
-	fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK);
-
-	if (fd < 0) {
-		printf("\tbaseboard eeprom: open fail\n");
-		return ERROR;
-	}
-
-	/* read data */
-	ret = read(fd, buf, sizeof(buf));
-	/* set last char to string termination */
-	buf[127] = '\0';
-
-	if (ret != sizeof(buf)) {
-		printf("\tbaseboard eeprom: ERROR: read fail\n", ret);
-		return ERROR;
-
-	} else {
-		printf("\tbaseboard eeprom: string: %s\n", (char *)buf);
-	}
-
-	close(fd);
-
-	/* XXX more tests here */
-
-	/* Let user know everything is ok */
-	printf("\tOK: baseboard eeprom passed all tests successfully\n");
-	return ret;
-}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_eeproms
- ****************************************************************************/
-
-int test_eeproms(int argc, char *argv[])
-{
-	unsigned	i;
-
-	printf("Running EEPROMs tests:\n\n");
-	fflush(stdout);
-
-	for (i = 0; eeproms[i].name; i++) {
-		printf("  eeprom: %s\n", eeproms[i].name);
-		eeproms[i].test(argc, argv);
-		fflush(stdout);
-		/* wait 100 ms to make sure buffer is emptied */
-		usleep(100000);
-	}
-
-	return 0;
-}

apps/px4/tests/test_float.c

@@ -45,8 +45,6 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
-#include <arch/board/board.h>
-#include <arch/board/drv_led.h>
 #include "tests.h"
 #include <math.h>
 #include <float.h>

apps/px4/tests/test_int.c

@@ -49,8 +49,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 #include <math.h>

apps/px4/tests/test_led.c

@@ -49,7 +49,7 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
+#include <drivers/drv_led.h>
 
 #include "tests.h"
 
@@ -91,7 +91,7 @@ int test_led(int argc, char *argv[])
 	int		fd;
 	int		ret = 0;
 
-	fd = open("/dev/led", O_RDONLY | O_NONBLOCK);
+	fd = open(LED_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		printf("\tLED: open fail\n");

apps/px4/tests/test_sleep.c

@@ -49,8 +49,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 /****************************************************************************

apps/px4/tests/test_uart_baudchange.c

@@ -52,8 +52,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 #include <math.h>

apps/px4/tests/test_uart_console.c

@@ -50,8 +50,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 #include <math.h>

apps/px4/tests/test_uart_loopback.c

@@ -50,8 +50,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 #include <math.h>

apps/px4/tests/test_uart_send.c

@@ -50,8 +50,6 @@
 
 #include <arch/board/board.h>
 
-#include <arch/board/drv_led.h>
-
 #include "tests.h"
 
 #include <math.h>

apps/px4/tests/tests.h

@@ -84,7 +84,6 @@ extern int	test_led(int argc, char *argv[]);
 extern int	test_adc(int argc, char *argv[]);
 extern int	test_int(int argc, char *argv[]);
 extern int	test_float(int argc, char *argv[]);
-extern int	test_eeproms(int argc, char *argv[]);
 extern int	test_ppm(int argc, char *argv[]);
 extern int	test_servo(int argc, char *argv[]);
 extern int	test_uart_loopback(int argc, char *argv[]);

apps/px4/tests/tests_main.c

@@ -97,7 +97,6 @@ struct {
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
 	{"adc",			test_adc,	OPT_NOJIGTEST, 0},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST, 0},
-	{"eeproms",		test_eeproms,	0, 0},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
 	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
 	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},

apps/systemcmds/led/led.c

@@ -1,209 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @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 led.c
- * Plain, stupid led outputs
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <math.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/drv_led.h>
-
-#include <systemlib/err.h>
-#include <systemlib/systemlib.h>
-
-__EXPORT int led_main(int argc, char *argv[]);
-
-
-static bool thread_should_exit = false;	/**< Deamon exit flag */
-static bool thread_running = false;	/**< Deamon status flag */
-static int led_task;			/**< Handle of deamon task / thread */
-static int leds;
-
-static int led_init(void)
-{
-	leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
-
-	if (leds < 0) {
-		errx(1, "[led] LED: open fail\n");
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		errx(1, "[led] LED: ioctl fail\n");
-	}
-
-	return 0;
-}
-
-static void led_deinit(void)
-{
-	close(leds);
-}
-
-static int led_toggle(int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-/**
- * Mainloop of led.
- */
-int led_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int led_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("led already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		led_task = task_spawn("led",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_MAX - 15,
-				      4096,
-				      led_thread_main,
-				      (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tled is running\n");
-
-		} else {
-			printf("\tled not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int led_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	printf("[led] Control started, taking over motors\n");
-
-	/* open leds */
-	led_init();
-
-	unsigned int rate = 200;
-
-	while (!thread_should_exit) {
-		/* swell blue led */
-
-
-		/* 200 Hz base loop */
-		usleep(1000000 / rate);
-	}
-
-	/* close leds */
-	led_deinit();
-
-	printf("[led] ending now...\n\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return OK;
-}
-

apps/systemcmds/param/param.c

@@ -56,36 +56,60 @@
 
 __EXPORT int param_main(int argc, char *argv[]);
 
-static void	do_save(void);
+static void	do_save(const char* param_file_name);
-static void	do_load(void);
+static void	do_load(const char* param_file_name);
-static void	do_import(void);
+static void	do_import(const char* param_file_name);
 static void	do_show(void);
 static void	do_show_print(void *arg, param_t param);
 
-static const char *param_file_name = "/eeprom/parameters";
-
 int
 param_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
-		if (!strcmp(argv[1], "save"))
+		if (!strcmp(argv[1], "save")) {
-			do_save();
+			if (argc >= 3) {
+				do_save(argv[2]);
+			} else {
+				do_save(param_get_default_file());
+			}
+		}
 
-		if (!strcmp(argv[1], "load"))
+		if (!strcmp(argv[1], "load")) {
-			do_load();
+			if (argc >= 3) {
+				do_load(argv[2]);
+			} else {
+				do_load(param_get_default_file());
+			}
+		}
 
-		if (!strcmp(argv[1], "import"))
+		if (!strcmp(argv[1], "import")) {
-			do_import();
+			if (argc >= 3) {
+				do_import(argv[2]);
+			} else {
+				do_import(param_get_default_file());
+			}
+		}
 
-		if (!strcmp(argv[1], "show"))
+		if (!strcmp(argv[1], "select")) {
+			if (argc >= 3) {
+				param_set_default_file(argv[2]);
+			} else {
+				param_set_default_file(NULL);
+			}
+			warnx("selected parameter file %s", param_get_default_file());
+		}
+
+		if (!strcmp(argv[1], "show")) {
 			do_show();
+		}
+			
 	}
 	
-	errx(1, "expected a command, try 'load', 'import', 'show' or 'save'\n");
+	errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'");
 }
 
 static void
-do_save(void)
+do_save(const char* param_file_name)
 {
 	/* delete the parameter file in case it exists */
 	unlink(param_file_name);
@@ -108,7 +132,7 @@ do_save(void)
 }
 
 static void
-do_load(void)
+do_load(const char* param_file_name)
 {
 	int fd = open(param_file_name, O_RDONLY);
 
@@ -118,14 +142,18 @@ do_load(void)
 	int result = param_load(fd);
 	close(fd);
 
-	if (result < 0)
+	if (result < 0) {
 		errx(1, "error importing from '%s'", param_file_name);
+	} else {
+		/* set default file name for next storage operation */
+		param_set_default_file(param_file_name);
+	}
 
 	exit(0);
 }
 
 static void
-do_import(void)
+do_import(const char* param_file_name)
 {
 	int fd = open(param_file_name, O_RDONLY);
 

apps/systemlib/geo/geo.c

@@ -49,19 +49,19 @@
 
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
-	double lat_now_rad = lat_now / 180.0 * M_PI;
+	double lat_now_rad = lat_now / 180.0d * M_PI;
-	double lon_now_rad = lon_now / 180.0 * M_PI;
+	double lon_now_rad = lon_now / 180.0d * M_PI;
-	double lat_next_rad = lat_next / 180.0 * M_PI;
+	double lat_next_rad = lat_next / 180.0d * M_PI;
-	double lon_next_rad = lon_next / 180.0 * M_PI;
+	double lon_next_rad = lon_next / 180.0d * M_PI;
 
 
 	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
-	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
+	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
-	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
-	const double radius_earth = 6371000.0;
+	const double radius_earth = 6371000.0d;
 
 	return radius_earth * c;
 }

apps/systemlib/param/param.c

@@ -47,6 +47,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <err.h>
+#include <errno.h>
 
 #include <sys/stat.h>
 
@@ -480,6 +481,83 @@ param_reset_all(void)
 	param_notify_changes();
 }
 
+static const char *param_default_file = "/eeprom/parameters";
+static char *param_user_file;
+
+int
+param_set_default_file(const char* filename)
+{
+	if (param_user_file != NULL) {
+		free(param_user_file);
+		param_user_file = NULL;
+	}
+	if (filename)
+		param_user_file = strdup(filename);
+	return 0;
+}
+
+const char *
+param_get_default_file(void)
+{
+	return (param_user_file != NULL) ? param_user_file : param_default_file;
+}
+
+int
+param_save_default(void)
+{
+	/* delete the file in case it exists */
+	unlink(param_get_default_file());
+
+	/* create the file */
+	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0) {
+		warn("opening '%s' for writing failed", param_get_default_file());
+		return -1;
+	}
+
+	int result = param_export(fd, false);
+	/* should not be necessary, over-careful here */
+	fsync(fd);
+	close(fd);
+
+	if (result != 0) {
+		unlink(param_get_default_file());
+		warn("error exporting parameters to '%s'", param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
+/**
+ * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
+ */
+int
+param_load_default(void)
+{
+	int fd = open(param_get_default_file(), O_RDONLY);
+
+	if (fd < 0) {
+		/* no parameter file is OK, otherwise this is an error */
+		if (errno != ENOENT) {
+			warn("open '%s' for reading failed", param_get_default_file());
+			return -1;
+		}
+		return 1;
+	}
+
+	int result = param_load(fd);
+	close(fd);
+
+	if (result != 0) {
+		warn("error reading parameters from '%s'", param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
 int
 param_export(int fd, bool only_unsaved)
 {

apps/systemlib/param/param.h

@@ -234,6 +234,40 @@ __EXPORT int		param_load(int fd);
  */
 __EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed);
 
+/**
+ * Set the default parameter file name.
+ *
+ * @param filename	Path to the default parameter file.  The file is not require to
+ *			exist.
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_set_default_file(const char* filename);
+
+/**
+ * Get the default parameter file name.
+ *
+ * @return		The path to the current default parameter file; either as
+ *			a result of a call to param_set_default_file, or the 
+ *			built-in default.
+ */
+__EXPORT const char 	*param_get_default_file(void);
+
+/**
+ * Save parameters to the default file.
+ *
+ * This function saves all parameters with non-default values.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_save_default(void);
+
+/**
+ * Load parameters from the default parameter file.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_load_default(void);
+
 /*
  * Macros creating static parameter definitions.
  *

apps/systemlib/pid/pid.c

@@ -155,8 +155,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
-	if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
+	if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
-	    fabs(i) > pid->intmax) {
+	    fabsf(i) > pid->intmax) {
 		i = pid->integral;		// If saturated then do not update integral value
 		pid->saturated = 1;
 

apps/systemlib/systemlib.c

@@ -45,7 +45,6 @@
 #include <signal.h>
 #include <sys/stat.h>
 #include <unistd.h>
-#include <arch/board/drv_eeprom.h>
 #include <float.h>
 #include <string.h>
 

apps/uORB/topics/vehicle_gps_position.h

@@ -66,6 +66,9 @@ struct vehicle_gps_position_s
 	uint16_t eph;				/**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME   */
 	uint16_t epv;				/**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535   */
 	uint16_t vel;				/**< GPS ground speed (m/s * 100). If unknown, set to: 65535   */
+	float vel_n;				/**< GPS ground speed in m/s */
+	float vel_e;				/**< GPS ground speed in m/s */
+	float vel_d;				/**< GPS ground speed in m/s */
 	uint16_t cog;				/**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535   */
 	uint8_t fix_type; 			/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 	uint8_t satellites_visible;		/**< Number of satellites visible. If unknown, set to 255   */
@@ -77,6 +80,9 @@ struct vehicle_gps_position_s
 	uint8_t satellite_snr[20];		/**< Signal to noise ratio of satellite   */
 	uint8_t satellite_info_available;	/**< 0 for no info, 1 for info available   */
 
+	/* flags */
+	float vel_ned_valid;			/**< Flag to indicate if NED speed is valid */
+
 };
 
 /**

nuttx/configs/px4fmu/nsh/appconfig

@@ -52,7 +52,6 @@ CONFIGURED_APPS += systemcmds/top
 CONFIGURED_APPS += systemcmds/boardinfo
 CONFIGURED_APPS += systemcmds/mixer
 CONFIGURED_APPS += systemcmds/eeprom
-CONFIGURED_APPS += systemcmds/led
 CONFIGURED_APPS += systemcmds/param
 CONFIGURED_APPS += systemcmds/bl_update
 #CONFIGURED_APPS += systemcmds/calibration
@@ -96,6 +95,7 @@ CONFIGURED_APPS += drivers/bma180
 CONFIGURED_APPS += drivers/l3gd20
 CONFIGURED_APPS	+= drivers/px4io
 CONFIGURED_APPS += drivers/stm32
+CONFIGURED_APPS += drivers/led
 CONFIGURED_APPS += drivers/stm32/tone_alarm
 CONFIGURED_APPS += drivers/px4fmu
 

nuttx/configs/px4fmu/src/Makefile

@@ -40,9 +40,7 @@ CFLAGS		+= -I$(TOPDIR)/sched
 ASRCS		= 
 AOBJS		= $(ASRCS:.S=$(OBJEXT))
 
-CSRCS		= up_leds.c \
+CSRCS		= empty.c
-		  drv_led.c drv_eeprom.c 
-
 COBJS		= $(CSRCS:.c=$(OBJEXT))
 
 SRCS		= $(ASRCS) $(CSRCS)
@@ -69,6 +67,7 @@ libboard$(LIBEXT): $(OBJS)
 	@( for obj in $(OBJS) ; do \
 		$(call ARCHIVE, $@, $${obj}); \
 	done ; )
+	touch $@
 
 .depend: Makefile $(SRCS)
 	@$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep

nuttx/configs/px4fmu/src/empty.c

@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */

nuttx/configs/px4fmu/src/px4fmu-internal.h

@@ -1,166 +0,0 @@
-/****************************************************************************************************
- * configs/px4fmu/src/px4fmu_internal.h
- * arch/arm/src/board/px4fmu_internal.n
- *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************************************/
-
-#ifndef __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H
-#define __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H
-
-/****************************************************************************************************
- * Included Files
- ****************************************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-/****************************************************************************************************
- * Definitions
- ****************************************************************************************************/
-/* Configuration ************************************************************************************/
-
-#ifdef CONFIG_STM32_SPI2
-#  error "SPI2 is not supported on this board"
-#endif
-
-#if defined(CONFIG_STM32_CAN1)
-#  warning "CAN1 is not supported on this board"
-#endif
-
-/* PX4FMU GPIOs ***********************************************************************************/
-/* LEDs */
-
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-
-/* External interrupts */
-#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
-
-/* SPI chip selects */
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
-
-/* User GPIOs
- *
- * GPIO0-1 are the buffered high-power GPIOs.
- * GPIO2-5 are the USART2 pins.
- * GPIO6-7 are the CAN2 pins.
- */
-#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
-#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
-#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-
-/* USB OTG FS
- *
- * 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)
-
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
- */
-
-#ifdef CONFIG_PWM
-#  if defined(CONFIG_STM32_TIM3_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#	 define BUZZER_PWMTIMER 3
-#  elif defined(CONFIG_STM32_TIM8_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#    define BUZZER_PWMTIMER 8
-#  endif
-#endif
-
-/****************************************************************************************************
- * Public Types
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Public data
- ****************************************************************************************************/
-
-#ifndef __ASSEMBLY__
-
-/****************************************************************************************************
- * Public Functions
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ****************************************************************************************************/
-
-extern void stm32_spiinitialize(void);
-
-/****************************************************************************************************
- * Name: px4fmu_gpio_init
- *
- * Description:
- *   Called to configure the PX4FMU user GPIOs
- *
- ****************************************************************************************************/
-
-extern void px4fmu_gpio_init(void);
-
-
-// XXX additional SPI chipselect functions required?
-
-#endif /* __ASSEMBLY__ */
-#endif /* __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H */
-

nuttx/configs/px4fmu/src/up_leds.c

@@ -1,127 +0,0 @@
-/****************************************************************************
- * configs/px4fmu/src/up_leds.c
- * arch/arm/src/board/up_leds.c
- *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-#include "up_internal.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-/* Enables debug output from this file (needs CONFIG_DEBUG with
- * CONFIG_DEBUG_VERBOSE too)
- */
-
-#undef LED_DEBUG  /* Define to enable debug */
-
-#ifdef LED_DEBUG
-#  define leddbg  lldbg
-#  define ledvdbg llvdbg
-#else
-#  define leddbg(x...)
-#  define ledvdbg(x...)
-#endif
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: up_ledinit
- ****************************************************************************/
-
-#ifdef CONFIG_ARCH_LEDS
-void up_ledinit(void)
-{
-   /* Configure LED1-2 GPIOs for output */
-
-   stm32_configgpio(GPIO_LED1);
-   stm32_configgpio(GPIO_LED2);
-}
-
-/****************************************************************************
- * Name: up_ledon
- ****************************************************************************/
-
-void up_ledon(int led)
-{
-	if (led == 0)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
-	}
-	if (led == 1)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED2, false);
-	}
-}
-
-/****************************************************************************
- * Name: up_ledoff
- ****************************************************************************/
-
-void up_ledoff(int led)
-{
-	if (led == 0)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
-	}
-	if (led == 1)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED2, true);
-	}
-}
-
-#endif /* CONFIG_ARCH_LEDS */