Merge branch 'master' into navigator_new_vector

ROMFS/px4fmu_common/init.d/rcS

@@ -75,14 +75,15 @@ then
 	#
 	# Load microSD params
 	#
-	#if ramtron start
+	if mtd start
-	#then
+	then
-	#	param select /ramtron/params
+		param select /fs/mtd_params
-	#	if [ -f /ramtron/params ]
+		if param load /fs/mtd_params
-	#	then
+		then
-	#		param load /ramtron/params
+		else
-	#	fi
+			echo "FAILED LOADING PARAMS"
-	#else
+		fi
+	else
 		param select /fs/microsd/params
 		if [ -f /fs/microsd/params ]
 		then
@@ -93,7 +94,7 @@ then
 				echo "Parameter file corrupt - ignoring"
 			fi
 		fi
-	#fi
+	fi
 	
 	#
 	# Start system state indicator

Tools/px4params/dokuwikiout.py

@@ -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 = name.replace("\n", "")
-                    name = "%s (%s)" % (name, code)
+                result += "| %s   | %s  " % (code, name)
-                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
+                    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 += "| %s  " % def_val
-                result += "\n"
+                else:
+                    result += "|"
+                long_desc = param.GetFieldValue("long_desc")
+                if long_desc is not None:
+                    long_desc = long_desc.replace("\n", "")
+                    result += "| %s  " % long_desc
+                else:
+                    result += "|"
+                result += "|\n"
+            result += "\n"
         return result

Tools/px4params/dokuwikiout_listings.py

@@ -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

makefiles/config_px4fmu-v2_default.mk

@@ -60,6 +60,7 @@ MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
 
 #
 # General system control

makefiles/config_px4fmu-v2_test.mk

@@ -22,6 +22,7 @@ MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
 
 #
 # Library modules

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1
 CONFIG_MMCSD_SPI=y
 CONFIG_MMCSD_SPICLOCK=24000000
 # CONFIG_MMCSD_SDIO is not set
-# CONFIG_MTD is not set
+CONFIG_MTD=y
 CONFIG_PIPES=y
 # CONFIG_PM is not set
 # CONFIG_POWER is not set
@@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
 # CONFIG_USART6_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
+#
+# MTD Configuration
+#
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+# CONFIG_MTD_RAMTRON is not set
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+
 #
 # USART1 Configuration
 #

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -500,8 +500,8 @@ CONFIG_MTD=y
 #
 # MTD Configuration
 #
-# CONFIG_MTD_PARTITION is not set
+CONFIG_MTD_PARTITION=y
-# CONFIG_MTD_BYTE_WRITE is not set
+CONFIG_MTD_BYTE_WRITE=y
 
 #
 # MTD Device Drivers

src/drivers/gps/gps.cpp

@@ -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) {
+		if (_fake_gps) {
-			delete(_Helper);
-			/* set to zero to ensure parser is not used while not instantiated */
-			_Helper = nullptr;
-		}
 
-		switch (_mode) {
+			_report.timestamp_position = hrt_absolute_time();
-		case GPS_DRIVER_MODE_UBX:
+			_report.lat = (int32_t)47.378301e7f;
-			_Helper = new UBX(_serial_fd, &_report);
+			_report.lon = (int32_t)8.538777e7f;
-			break;
+			_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:
+			//no time and satellite information simulated
-			_Helper = new MTK(_serial_fd, &_report);
-			break;
 
-		default:
+			if (_report_pub > 0) {
-			break;
+				orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
-		}
 
-		unlock();
+			} else {
-
+				_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-		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) {
+			usleep(2e5);
-				warnx("module lost");
+
-				_healthy = false;
+		} else {
-				_rate = 0.0f;
+
+			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 */
+			case GPS_DRIVER_MODE_MTK:
-		switch (_mode) {
+				_mode = GPS_DRIVER_MODE_UBX;
-		case GPS_DRIVER_MODE_UBX:
+				break;
-			_mode = GPS_DRIVER_MODE_MTK;
-			break;
 
-		case GPS_DRIVER_MODE_MTK:
+			default:
-			_mode = GPS_DRIVER_MODE_UBX;
+				break;
-			break;
+			}
-
-		default:
-			break;
 		}
 
 	}
@@ -417,7 +454,7 @@ namespace gps
 
 GPS	*g_dev;
 
-void	start(const char *uart_path);
+void	start(const char *uart_path, bool fake_gps);
 void	stop();
 void	test();
 void	reset();
@@ -427,7 +464,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path)
+start(const char *uart_path, bool fake_gps)
 {
 	int fd;
 
@@ -435,7 +472,7 @@ start(const char *uart_path)
 		errx(1, "already started");
 
 	/* create the driver */
-	g_dev = new GPS(uart_path);
+	g_dev = new GPS(uart_path, fake_gps);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -527,6 +564,7 @@ gps_main(int argc, char *argv[])
 
 	/* set to default */
 	char *device_name = GPS_DEFAULT_UART_PORT;
+	bool fake_gps = false;
 
 	/*
 	 * Start/load the driver.
@@ -542,7 +580,13 @@ gps_main(int argc, char *argv[])
 			}
 		}
 
-		gps::start(device_name);
+		/* Detect fake gps option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-f"))
+				fake_gps = true;
+		}
+
+		gps::start(device_name, fake_gps);
 	}
 
 	if (!strcmp(argv[1], "stop"))
@@ -567,5 +611,5 @@ gps_main(int argc, char *argv[])
 		gps::info();
 
 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
 }

src/drivers/hil/hil.cpp

@@ -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 */
+	// XXX already claimed with CDEV
-	if (_primary_pwm_device)
+	// /* clean up the alternate device node */
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// if (_primary_pwm_device)
+	// 	unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	g_hil = nullptr;
 }

src/drivers/rgbled/rgbled.cpp

@@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 void
 rgbled_usage()
 {
-	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
 	warnx("    -a addr (0x%x)", ADDR);
@@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[])
 	if (g_rgbled == nullptr) {
 		warnx("not started");
 		rgbled_usage();
-		exit(0);
+		exit(1);
 	}
 
 	if (!strcmp(verb, "test")) {
@@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(verb, "off")) {
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
 
 		if (fd == -1) {
@@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[])
 		exit(ret);
 	}
 
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		exit(0);
+	}
+
 	if (!strcmp(verb, "rgb")) {
 		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");

src/modules/commander/commander.cpp

@@ -106,14 +106,9 @@ static const int ERROR = -1;
 
 extern struct system_load_s system_load;
 
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
 /* Decouple update interval and hysteris counters, all depends on intervals */
 #define COMMANDER_MONITORING_INTERVAL 50000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
 #define MAVLINK_OPEN_INTERVAL 50000
 
@@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Start monitoring loop */
 	unsigned counter = 0;
-	unsigned low_voltage_counter = 0;
-	unsigned critical_voltage_counter = 0;
 	unsigned stick_off_counter = 0;
 	unsigned stick_on_counter = 0;
 
@@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[])
 	int battery_sub = orb_subscribe(ORB_ID(battery_status));
 	struct battery_status_s battery;
 	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;
 
 	/* Subscribe to subsystem info topic */
 	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -888,14 +880,12 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-
+			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			// warnx("bat v: %2.2f", battery.voltage_v);
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
-
+				status.battery_voltage = battery.voltage_filtered_v;
-			/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
+				status.battery_current = battery.current_a;
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
-				status.battery_voltage = battery.voltage_v;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
 			}
 		}
 
@@ -948,46 +938,29 @@ int commander_thread_main(int argc, char *argv[])
 			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
-		// XXX remove later
-		//warnx("bat remaining: %2.2f", status.battery_remaining);
-
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
-			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+			low_battery_voltage_actions_done = true;
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
-				low_battery_voltage_actions_done = true;
+			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
-				mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+			status_changed = true;
-				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+			battery_tune_played = false;
-				status_changed = true;
-				battery_tune_played = false;
-			}
-
-			low_voltage_counter++;
 
 		} 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;
-				critical_battery_voltage_actions_done = true;
+			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
-				mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+			battery_tune_played = false;
-				battery_tune_played = false;
 
-				if (armed.armed) {
+			if (armed.armed) {
-					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 
-				} else {
+			} else {
-					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
-				}
-
-				status_changed = true;
 			}
 
-			critical_voltage_counter++;
+			status_changed = true;
-
-		} else {
-
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
 		}
 
 		/* End battery voltage check */

src/modules/commander/commander_helper.cpp

@@ -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_v_empty_h;
-	static param_t bat_volt_full;
+	static param_t bat_v_full_h;
-	static param_t bat_n_cells;
+	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_v_empty_h = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
+		bat_v_full_h = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
+		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_v_empty_h, &bat_v_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
+		param_get(bat_v_full_h, &bat_v_full);
-		param_get(bat_n_cells, &ncells);
+		param_get(bat_n_cells_h, &bat_n_cells);
+		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	counter++;
 
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+	/* remaining charge estimate based on voltage */
+	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+
+	if (bat_capacity > 0.0f) {
+		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
+		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
+	} else {
+		/* else use voltage */
+		ret = remaining_voltage;
+	}
 
 	/* limit to sane values */
 	ret = (ret < 0.0f) ? 0.0f : ret;

src/modules/commander/commander_helper.h

@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
 void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
 /**
- * Provides a coarse estimate of remaining battery power.
+ * Estimate remaining battery charge.
  *
- * The estimate is very basic and based on decharging voltage curves.
+ * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
+ * else use simple estimate based on voltage.
  *
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage);
+float battery_remaining_estimate_voltage(float voltage, float discharged);
 
 #endif /* COMMANDER_HELPER_H_ */

src/modules/commander/commander_params.c

@@ -48,6 +48,7 @@
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -49,21 +48,75 @@
  *
  */
 
+/**
+ * L1 period
+ *
+ * This is the L1 distance and defines the tracking
+ * point ahead of the aircraft its following.
+ * A value of 25 meters works for most aircraft. Shorten
+ * slowly during tuning until response is sharp without oscillation.
+ *
+ * @min 1.0
+ * @max 100.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
 
-
+/**
+ * L1 damping
+ *
+ * Damping factor for L1 control.
+ *
+ * @min 0.6
+ * @max 0.9
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 
-
+/**
+ * Default Loiter Radius
+ *
+ * This radius is used when no other loiter radius is set.
+ *
+ * @min 10.0
+ * @max 100.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
 
-
+/**
+ * Cruise throttle
+ *
+ * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
 
-
+/**
+ * Negative pitch limit
+ *
+ * The minimum negative pitch the controller will output.
+ *
+ * @unit degrees
+ * @min -60.0
+ * @max 0.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
 
-
+/**
+ * Positive pitch limit
+ *
+ * The maximum positive pitch the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @max 60.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
 

src/modules/mavlink/mavlink.c

@@ -680,8 +680,8 @@ int mavlink_thread_main(int argc, char *argv[])
 						    v_status.onboard_control_sensors_health,
 						    v_status.load * 1000.0f,
 						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 1000.0f,
+						    v_status.battery_current * 100.0f,
-						    v_status.battery_remaining,
+						    v_status.battery_remaining * 100.0f,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
 						    v_status.errors_count1,

src/modules/sdlog2/sdlog2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *           Anton Babushkin <anton.babushkin@me.com>
  *
@@ -704,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
+		struct battery_status_s battery;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -729,6 +730,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int airspeed_sub;
 		int esc_sub;
 		int global_vel_sp_sub;
+		int battery_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -755,6 +757,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
+			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
 		} body;
 	} log_msg = {
@@ -764,9 +767,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 */
+	/* number of messages */
-	const ssize_t fdsc = 20;
+	const ssize_t fdsc = 25;
-	/* sanity check variable and index */
+	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
 	struct pollfd fds[fdsc];
@@ -891,6 +894,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.
@@ -986,8 +995,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
 				log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
-				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
-				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1264,6 +1271,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GVSP);
 			}
 
+			/* --- BATTERY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
+				log_msg.msg_type = LOG_BATT_MSG;
+				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
+				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
+				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
+				LOGBUFFER_WRITE_AND_COUNT(BATT);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */

src/modules/sdlog2/sdlog2_messages.h

@@ -148,8 +148,6 @@ struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t navigation_state;
 	uint8_t arming_state;
-	float battery_voltage;
-	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -247,8 +245,17 @@ 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;
+};
+
 /* --- DIST - DISTANCE TO SURFACE --- */
-#define LOG_DIST_MSG 20
+#define LOG_DIST_MSG 21
 struct log_DIST_s {
 	float bottom;
 	float bottom_rate;
@@ -290,7 +297,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"),
@@ -300,6 +307,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless

src/modules/sensors/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

src/modules/sensors/sensor_params.c

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 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>
  *
  * 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.
+ * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
- * 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);
+ * RC Channel 2 Minimum
-PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
+ *
+ * Minimum value for RC channel 2
+ *
+ * @min 800.0
+ * @max 1500.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
+
+/**
+ * RC Channel 2 Trim
+ *
+ * Mid point value (same as min for throttle)
+ *
+ * @min 800.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
+
+/**
+ * RC Channel 2 Maximum
+ *
+ * Maximum value for RC channel 2
+ *
+ * @min 1500.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
+
+/**
+ * RC Channel 2 Reverse
+ *
+ * Set to -1 to reverse channel.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
+
+/**
+ * RC Channel 2 dead zone
+ *
+ * The +- range of this value around the trim value will be considered as zero.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
@@ -223,6 +379,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
+PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);

src/modules/sensors/sensors.cpp

@@ -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 BATT_V_LOWPASS 0.001f
-#define BAT_VOL_LOWPASS_1 0.99f
+#define BATT_V_IGNORE_THRESHOLD 3.5f
-#define BAT_VOL_LOWPASS_2 0.01f
-#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
 
 /**
  * HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
 	math::Matrix<3,3>	_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;
@@ -465,7 +469,9 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_mag_is_external(false)
+	_mag_is_external(false),
+	_battery_discharged(0),
+	_battery_current_timestamp(0)
 {
 
 	/* basic r/c parameters */
@@ -558,6 +564,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");
@@ -738,6 +745,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));
 
@@ -1155,17 +1167,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])) {
-
+			for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-			if (ret >= (int)sizeof(buf_adc[0])) {
-
 				/* 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);
@@ -1176,27 +1187,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) {
+						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
-							_battery_status.voltage_v = voltage;
+							_battery_status.voltage_filtered_v = voltage;
 						}
 
-						_battery_status.timestamp = hrt_absolute_time();
+						_battery_status.timestamp = t;
-						_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
+						_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
-						/* current and discharge are unknown */
-						_battery_status.current_a = -1.0f;
-						_battery_status.discharged_mah = -1.0f;
 
-						/* announce the battery voltage if needed, just publish else */
+					} else {
-						if (_battery_pub > 0) {
+						/* mark status as invalid */
-							orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+						_battery_status.voltage_v = -1.0f;
+						_battery_status.voltage_filtered_v = -1.0f;
+					}
 
-						} else {
+				} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
-							_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+					/* 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) {
 
@@ -1212,7 +1236,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;
 
@@ -1225,8 +1249,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);
+				}
 			}
 		}
 	}
@@ -1514,7 +1546,10 @@ Sensors::task_main()
 	raw.adc_voltage_v[3] = 0.0f;
 
 	memset(&_battery_status, 0, sizeof(_battery_status));
-	_battery_status.voltage_v = BAT_VOL_INITIAL;
+	_battery_status.voltage_v = 0.0f;
+	_battery_status.voltage_filtered_v = 0.0f;
+	_battery_status.current_a = -1.0f;
+	_battery_status.discharged_mah = -1.0f;
 
 	/* get a set of initial values */
 	accel_poll(raw);

src/modules/systemlib/bson/tinybson.c

@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
 		memcpy(encoder->buf, &len, sizeof(len));
 	}
 
+	/* sync file */
+	fsync(encoder->fd);
+
 	return 0;
 }
 

src/modules/systemlib/param/param.c

@@ -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);
+	int result = param_load(fd_load);
-	close(fd);
+	close(fd_load);
 
 	if (result != 0) {
 		warn("error reading parameters from '%s'", param_get_default_file());

src/modules/uORB/topics/battery_status.h

@@ -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   	voltage_v;		/**< Battery voltage in volts, 0 if unknown           	 */
-	float		current_a;		/**< Battery current in amperes, filtered, -1 if unknown */
+	float   	voltage_filtered_v;		/**< Battery voltage in volts, filtered, 0 if unknown  	 */
-	float		discharged_mah;		/**< Discharged amount in mAh, filtered, -1 if unknown	 */
+	float		current_a;		/**< Battery current in amperes, -1 if unknown */
+	float		discharged_mah;		/**< Discharged amount in mAh, -1 if unknown	 */
 };
 
 /**
@@ -65,4 +66,4 @@ struct battery_status_s {
 /* register this as object request broker structure */
 ORB_DECLARE(battery_status);
 
-#endif
+#endif

src/systemcmds/mtd/module.mk

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

src/systemcmds/mtd/mtd.c

@@ -0,0 +1,279 @@
+/****************************************************************************
+ *
+ *   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"
+
+__EXPORT int mtd_main(int argc, char *argv[]);
+
+#ifndef CONFIG_MTD_RAMTRON
+
+/* create a fake command with decent warnx to not confuse users */
+int mtd_main(int argc, char *argv[])
+{
+	errx(1, "RAMTRON not enabled, skipping.");
+}
+
+#else
+
+static void	mtd_attach(void);
+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 bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *mtd_dev;
+
+/* 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], "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);
+
+static void
+mtd_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 MTD 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;
+}
+
+static void
+mtd_start(char *partition_names[], unsigned n_partitions)
+{
+	int ret;
+
+	if (started)
+		errx(1, "mtd already mounted");
+
+	if (!attached)
+		mtd_attach();
+
+	if (!mtd_dev) {
+		warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
+		exit(1);
+	}
+
+
+	/* Get the geometry of the FLASH device */
+
+	FAR struct mtd_geometry_s geo;
+
+	ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
+
+	if (ret < 0) {
+		warnx("ERROR: mtd->ioctl failed: %d", ret);
+		exit(3);
+	}
+
+	warnx("Flash Geometry:");
+	warnx("  blocksize:      %lu", (unsigned long)geo.blocksize);
+	warnx("  erasesize:      %lu", (unsigned long)geo.erasesize);
+	warnx("  neraseblocks:   %lu", (unsigned long)geo.neraseblocks);
+
+	/* Determine the size of each partition.  Make each partition an even
+	 * multiple of the erase block size (perhaps not using some space at the
+	 * end of the FLASH).
+	 */
+
+	unsigned blkpererase = geo.erasesize / geo.blocksize;
+	unsigned nblocks     = (geo.neraseblocks / n_partitions) * blkpererase;
+	unsigned partsize    = nblocks * geo.blocksize;
+
+	warnx("  No. partitions: %u", n_partitions);
+	warnx("  Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize);
+
+	/* Now create MTD FLASH partitions */
+
+	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);
+		}
+	}
+
+	started = true;
+	exit(0);
+}
+
+static void
+mtd_test(void)
+{
+	warnx("This test routine does not test anything yet!");
+	exit(1);
+}
+
+static 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

src/systemcmds/param/param.c

@@ -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))))) {
+	if (!(arg == NULL)) {
-		/* param not found */
+
-		return;
+		/* start search */
+		char *ss = search_string;
+		char *pp = p_name;
+		bool mismatch = false;
+
+		/* XXX this comparison is only ok for trailing wildcards */
+		while (*ss != '\0' && *pp != '\0') {
+
+			if (*ss == *pp) {
+				ss++;
+				pp++;
+			} else if (*ss == '*') {
+				if (*(ss + 1) != '\0') {
+					warnx("* symbol only allowed at end of search string.");
+					exit(1);
+				}
+
+				pp++;
+			} else {
+				/* param not found */
+				return;
+			}
+		}
+
+		/* the search string must have been consumed */
+		if (!(*ss == '\0' || *ss == '*'))
+			return;
 	}
 
 	printf("%c %s: ",

src/systemcmds/tests/test_file.c

@@ -54,9 +54,9 @@
 
 #include "tests.h"
 
-int check_user_abort();
+int check_user_abort(int fd);
 
-int check_user_abort() {
+int check_user_abort(int fd) {
 	/* check if user wants to abort */
 	char c;
 
@@ -77,6 +77,8 @@ int check_user_abort() {
 		case 'q':
 		{
 			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
 			return OK;
 			/* not reached */
 			}
@@ -141,7 +143,7 @@ test_file(int argc, char *argv[])
 
 				fsync(fd);
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -175,7 +177,7 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -199,7 +201,7 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 
-				if (!check_user_abort())
+				if (!check_user_abort(fd))
 					return OK;
 
 			}
@@ -232,7 +234,7 @@ test_file(int argc, char *argv[])
 						break;
 					}
 
-					if (!check_user_abort())
+					if (!check_user_abort(fd))
 						return OK;
 				}
 
@@ -275,7 +277,7 @@ test_file(int argc, char *argv[])
 							break;
 					}
 
-					if (!check_user_abort())
+					if (!check_user_abort(fd))
 						return OK;
 				}