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

13 files changed, 299 insertions, 98 deletions

diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 2a734c27e..26222c255 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
-# CONFIG_UART7_RXDMA is not set
+CONFIG_UART7_RXDMA=y
 # CONFIG_UART8_RS485 is not set
 CONFIG_UART8_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
@@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
 # CONFIG_USART3_SERIAL_CONSOLE is not set
 # CONFIG_UART4_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-# CONFIG_UART7_SERIAL_CONSOLE is not set
-CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART7_SERIAL_CONSOLE=y
+# CONFIG_UART8_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index e0f207696..aa3c5000a 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,7 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
-#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file
+#define ecl_elapsed_time hrt_elapsed_time
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 3b68a0a4e..d1c864d78 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -38,6 +38,8 @@
  *
  */
 
+#include <float.h>
+
 #include "ecl_l1_pos_controller.h"
 
 float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
 	/* calculate the vector from waypoint A to current position */
 	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
-	/* store the normalized vector from waypoint A to current position */
-	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit;
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
 
 	/* calculate eta angle towards the loiter center */
 
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0f28bccad..3730b1920 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -3,13 +3,10 @@
 #include "tecs.h"
 #include <ecl/ecl.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 using namespace math;
 
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
 /**
  * @file tecs.cpp
  *
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index d1ebacda1..5cafb1c79 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,16 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
-
-		_airspeed_enabled(false),
-		_throttle_slewrate(0.0f),
-		_climbOutDem(false),
-		_hgt_dem_prev(0.0f),
-		_hgt_dem_adj_last(0.0f),
-		_hgt_dem_in_old(0.0f),
-		_TAS_dem_last(0.0f),
-		_TAS_dem_adj(0.0f),
-		_TAS_dem(0.0f),
+		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
 		_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
 		_integ5_state(0.0f),
 		_integ6_state(0.0f),
 		_integ7_state(0.0f),
-		_pitch_dem(0.0f),
 		_last_pitch_dem(0.0f),
+		_vel_dot(0.0f),
+		_TAS_dem(0.0f),
+		_TAS_dem_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_prev(0.0f),
+		_TAS_dem_adj(0.0f),
+		_STEdotErrLast(0.0f),
+		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
 		_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
-		_vel_dot(0.0f),
-		_STEdotErrLast(0.0f) {
-
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f)
+	{
 	}
 
 	bool airspeed_sensor_enabled() {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d5af63120..d114a2e5c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
-			if (safety.safety_switch_available && !safety.safety_off && armed.armed) {
+			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
@@ -962,7 +962,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 64d2b7019..ed6d8792c 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -130,23 +130,23 @@ private:
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_control_mode_sub;			/**< vehicle status subscription */
+	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_sensor_combined_sub;			/**< for body frame accelerations */
+	int		_sensor_combined_sub;		/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
-	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
+	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
+	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
+	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
+	struct airspeed_s				_airspeed;			/**< airspeed */
+	struct vehicle_control_mode_s			_control_mode;			/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
+	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -154,13 +154,13 @@ private:
 
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
+	double		_loiter_hold_lat;
+	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
 	bool		_loiter_hold;
 
-	float		_launch_lat;
-	float		_launch_lon;
+	double		_launch_lat;
+	double		_launch_lon;
 	float		_launch_alt;
 	bool		_launch_valid;
 
@@ -192,7 +192,7 @@ private:
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Matrix<3, 3> _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -360,6 +360,7 @@ FixedwingPositionControl	*g_control;
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
+	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_control_task(-1),
 
@@ -378,38 +379,33 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
 /* states */
 	_setpoint_valid(false),
 	_loiter_hold(false),
-	_airspeed_error(0.0f),
-	_airspeed_valid(false),
-	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
+	launch_detected(false),
+	usePreTakeoffThrust(false),
 	flare_curve_alt_last(0.0f),
-	_mavlink_fd(-1),
 	launchDetector(),
-	launch_detected(false),
-	usePreTakeoffThrust(false)
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false),
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined()
 {
-	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	navigation_capabilities_s		_nav_capabilities = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_control_mode = {0};
-	vehicle_global_position_s		_global_pos = {0};
-	position_setpoint_triplet_s			_pos_sp_triplet = {0};
-	sensor_combined_s				_sensor_combined = {0};
-
-
-
-
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -583,8 +579,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 
 		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat / 1e7f;
-			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_lat = _global_pos.lat;
+			_launch_lon = _global_pos.lon;
 			_launch_alt = _global_pos.alt;
 			_launch_valid = true;
 		}
@@ -699,7 +695,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid) {
+	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -1058,13 +1054,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
 		}
 
-		/* climb out control */
-		bool climb_out = false;
+		//XXX not used
 
-		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-			climb_out = true;
-		}
+		/* climb out control */
+//		bool climb_out = false;
+//
+//		/* user wants to climb out */
+//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//			climb_out = true;
+//		}
 
 		/* if in seatbelt mode, set airspeed based on manual control */
 
@@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1c040f1ff..16eca8d79 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -154,17 +154,16 @@ private:
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
-	orb_advert_t	_pos_sp_triplet_pub;			/**< publish position setpoint triplet */
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct home_position_s				_home_pos;		/**< home position for RTL */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< triplet of position setpoints */
 	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
-	struct mission_item_s				_mission_item;	/**< current mission item */
-	bool		_mission_item_valid;	/**< current mission item valid */
+	struct mission_item_s				_mission_item;		/**< current mission item */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -178,21 +177,22 @@ private:
 
 	class 		Mission				_mission;
 
-	bool		_global_pos_valid;				/**< track changes of global_position.global_valid flag */
-	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
+	bool		_mission_item_valid;		/**< current mission item valid */
+	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
+	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_need_takeoff;		/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
-	bool		_do_takeoff;		/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
+	bool		_need_takeoff;			/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
+	bool		_do_takeoff;			/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
 
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
+	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
 
 	bool		_pos_sp_triplet_updated;
 
-	char *nav_states_str[NAV_STATE_MAX];
+	const char *nav_states_str[NAV_STATE_MAX];
 
 	struct {
 		float min_altitude;
@@ -387,11 +387,11 @@ Navigator::Navigator() :
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_control_mode_sub(-1),
 	_params_sub(-1),
 	_offboard_mission_sub(-1),
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
+	_control_mode_sub(-1),
 
 /* publications */
 	_pos_sp_triplet_pub(-1),
@@ -400,22 +400,22 @@ Navigator::Navigator() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 
-/* states */
-	_rtl_state(RTL_STATE_NONE),
+	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_mission_item_valid(false),
 	_global_pos_valid(false),
 	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_set_nav_state_timestamp(0),
-	_mission_item_valid(false),
 	_need_takeoff(true),
 	_do_takeoff(false),
+	_set_nav_state_timestamp(0),
 	_pos_sp_triplet_updated(false),
-	_geofence_violation_warning_sent(false)
+/* states */
+	_rtl_state(RTL_STATE_NONE)
 {
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@@ -1167,7 +1167,7 @@ Navigator::set_mission_item()
 		}
 
 		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
 
 		} else {
 			if (onboard) {
@@ -1326,7 +1326,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
 			break;
 		}
 
@@ -1359,7 +1359,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
@@ -1377,7 +1377,7 @@ Navigator::set_rtl_item()
 			_mission_item.loiter_direction = 1;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
+			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
 			_mission_item.pitch_min = 0.0f;
 			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -1386,7 +1386,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
@@ -1552,7 +1552,7 @@ Navigator::check_mission_item_reached()
 			_time_first_inside_orbit = now;
 
 			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside);
+				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
 			}
 		}
 
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 6a4429461..dfa98d425 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -327,7 +327,7 @@ mixer_handle_text(const void *buffer, size_t length)
 		return 1;
 	}
 
-	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+	px4io_mixdata	*msg = static_cast<px4io_mixdata *>(buffer);
 
 	isr_debug(2, "mix txt %u", length);
 
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index acf28c35b..622a0faf3 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -26,6 +26,7 @@ SRCS			 = test_adc.c \
 			   test_mixer.cpp \
 			   test_mathlib.cpp \
 			   test_file.c \
+			   test_file2.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c
new file mode 100644
index 000000000..ef555f6c3
--- /dev/null
+++ b/src/systemcmds/tests/test_file2.c
@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   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 PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_file2.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#define FLAG_FSYNC 1
+#define FLAG_LSEEK 2
+
+/*
+  return a predictable value for any file offset to allow detection of corruption
+ */
+static uint8_t get_value(uint32_t ofs)
+{
+	union {
+		uint32_t ofs;
+		uint8_t buf[4];
+	} u;
+	u.ofs = ofs;
+	return u.buf[ofs % 4];
+}
+
+static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
+{
+	printf("Testing on %s with write_chunk=%u write_size=%u\n", 
+	       filename, (unsigned)write_chunk, (unsigned)write_size);
+	
+	uint32_t ofs = 0;
+	int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+	
+	// create a file of size write_size, in write_chunk blocks
+	uint8_t counter = 0;
+	while (ofs < write_size) {
+        uint8_t buffer[write_chunk];
+        for (uint16_t j=0; j<write_chunk; j++) {
+		buffer[j] = get_value(ofs);
+		ofs++;
+        }
+        if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+		printf("write failed at offset %u\n", ofs);
+		exit(1);
+        }
+	if (flags & FLAG_FSYNC) {
+		fsync(fd);
+	}
+        if (counter % 100 == 0) {
+		printf("write ofs=%u\r", ofs);
+        }
+        counter++;
+	}
+	close(fd);
+	
+	printf("write ofs=%u\n", ofs);
+	
+	// read and check
+	fd = open(filename, O_RDONLY);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+
+	counter = 0;
+	ofs = 0;
+	while (ofs < write_size) {
+		uint8_t buffer[write_chunk];
+		if (counter % 100 == 0) {
+			printf("read ofs=%u\r", ofs);
+		}
+		counter++;
+		if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+			printf("read failed at offset %u\n", ofs);
+			exit(1);
+		}
+		for (uint16_t j=0; j<write_chunk; j++) {
+			if (buffer[j] != get_value(ofs)) {
+				printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
+				exit(1);
+			}
+			ofs++;
+		}
+		if (flags & FLAG_LSEEK) {
+			lseek(fd, 0, SEEK_CUR);
+		}
+	}
+	printf("read ofs=%u\n", ofs);
+	close(fd);
+	unlink(filename);
+	printf("All OK\n");    
+}
+
+static void usage(void)
+{
+	printf("test file2 [options] [filename]\n");
+	printf("\toptions:\n");
+	printf("\t-s SIZE     set file size\n");
+	printf("\t-c CHUNK    set IO chunk size\n");
+	printf("\t-F          fsync on every write\n");
+	printf("\t-L          lseek on every read\n");
+}
+
+int test_file2(int argc, char *argv[])
+{
+	int opt;
+	uint16_t flags = 0;
+	const char *filename = "/fs/microsd/testfile2.dat";
+	uint32_t write_chunk = 64;
+	uint32_t write_size = 5*1024;
+
+	while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
+		switch (opt) {
+		case 'F':
+			flags |= FLAG_FSYNC;
+			break;
+		case 'L':
+			flags |= FLAG_LSEEK;
+			break;
+		case 's':
+			write_size = strtoul(optarg, NULL, 0);
+			break;
+		case 'c':
+			write_chunk = strtoul(optarg, NULL, 0);
+			break;
+		case 'h':
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (argc > 0) {
+		filename = argv[0];
+	}
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+        test_corruption(filename, write_chunk, write_size, flags);
+        return 0;
+}
+
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index ac64ad33d..ad55e1410 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -107,6 +107,7 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_file2(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 73827b7cf..fe22f6177 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -104,6 +104,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"file2",		test_file2,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},