Ported almost everything to new param interface, ready for serious testing

9 files changed, 296 insertions, 352 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index e89dae983..e2d197648 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -68,6 +68,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/subsystem_info.h>
 #include <mavlink/mavlink_log.h>
  
 #include <systemlib/param/param.h>
@@ -108,7 +109,7 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 
 /* pthread loops */
 static void *command_handling_loop(void *arg);
-// static void *subsystem_info_loop(void *arg);
+static void *orb_receive_loop(void *arg);
 
 __EXPORT int commander_main(int argc, char *argv[]);
 
@@ -137,6 +138,16 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
  */
 static void usage(const char *reason);
 
+/**
+ * Sort calibration values.
+ *
+ * Sorts the calibration values with bubble sort.
+ *
+ * @param a 	The array to sort
+ * @param n 	The number of entries in the array
+ */
+static void cal_bsort(int16_t a[], int n);
+
 static int buzzer_init()
 {
 	buzzer = open("/dev/tone_alarm", O_WRONLY);
@@ -227,7 +238,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
 	return 0;
 }
 
-void cal_bsort(int16_t a[], int n)
+static void cal_bsort(int16_t a[], int n)
 {
 	int i,j,t;
 	for(i=0;i<n-1;i++)
@@ -465,9 +476,17 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status
 	gyro_offset[1] = gyro_offset[1] / calibration_count;
 	gyro_offset[2] = gyro_offset[2] / calibration_count;
 
-	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET] = gyro_offset[0];
-	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET] = gyro_offset[1];
-	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2];
+	if (param_set(param_find("SENSOR_GYRO_XOFF"), &(gyro_offset[0]))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
+	}
+	
+	if (param_set(param_find("SENSOR_GYRO_YOFF"), &(gyro_offset[1]))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!");
+	}
+
+	if (param_set(param_find("SENSOR_GYRO_ZOFF"), &(gyro_offset[2]))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
+	}
 
 	char offset_output[50];
 	sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
@@ -625,6 +644,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0) {
 		/* send acknowledge command */
+		// XXX TODO
 	}
 
 }
@@ -641,7 +661,7 @@ static void *command_handling_loop(void *arg)
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	struct vehicle_command_s cmd;
 
-	while (1) {
+	while (!thread_should_exit) {
 		struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
 
 		if (poll(fds, 1, 5000) == 0) {
@@ -655,71 +675,37 @@ static void *command_handling_loop(void *arg)
 		}
 	}
 
+	close(cmd_sub);
+
 	return NULL;
 }
 
-// static void *subsystem_info_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-// {
-// 	/* Set thread name */
-// 	prctl(PR_SET_NAME, "commander subsys", getpid());
-
-// 	uint8_t current_info_local = SUBSYSTEM_INFO_BUFFER_SIZE;
-// 	uint16_t total_counter = 0;
-
-// 	while (1) {
-
-// 		if (0 == global_data_wait(&global_data_subsystem_info->access_conf)) {
-// //				printf("got subsystem_info\n");
-
-// 			while (current_info_local != global_data_subsystem_info->current_info) {
-// //					printf("current_info_local = %d, current_info = %d \n", current_info_local, global_data_subsystem_info->current_info);
-
-// 				current_info_local++;
-
-// 				if (current_info_local >= SUBSYSTEM_INFO_BUFFER_SIZE)
-// 					current_info_local = 0;
-
-// 				/* Handle the new subsystem info and write updated version of global_data_sys_status */
-// 				subsystem_info_t *info = &(global_data_subsystem_info->info[current_info_local]);
-
-// //					printf("Commander got subsystem info: %d %d %d\n", info->present, info->enabled, info->health);
-
-
-// 				if (info->present != 0) {
-// 					update_state_machine_subsystem_present(stat_pub, &current_status, &info->subsystem_type);
-
-// 				} else {
-// 					update_state_machine_subsystem_notpresent(stat_pub, &current_status, &info->subsystem_type);
-// 				}
-
-// 				if (info->enabled != 0) {
-// 					update_state_machine_subsystem_enabled(stat_pub, &current_status, &info->subsystem_type);
-
-// 				} else {
-// 					update_state_machine_subsystem_disabled(stat_pub, &current_status, &info->subsystem_type);
-// 				}
+static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander orb rcv", getpid());
 
-// 				if (info->health != 0) {
-// 					update_state_machine_subsystem_healthy(stat_pub, &current_status, &info->subsystem_type);
+	/* Subscribe to command topic */
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	struct subsystem_info_s info;
 
-// 				} else {
-// 					update_state_machine_subsystem_unhealthy(stat_pub, &current_status, &info->subsystem_type);
-// 				}
+	while (!thread_should_exit) {
+		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
 
-// 				total_counter++;
-// 			}
+		if (poll(fds, 1, 5000) == 0) {
+			/* timeout, but this is no problem, silently ignore */
+		} else {
+			/* got command */
+			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
 
-// 			if (global_data_subsystem_info->counter - total_counter > SUBSYSTEM_INFO_BUFFER_SIZE) {
-// 				printf("[commander] Warning: Too many subsystem status updates, subsystem_info buffer full\n"); //TODO: add to error queue
-// 				global_data_subsystem_info->counter = total_counter; //this makes sure we print the warning only once
-// 			}
+			printf("Subsys changed: %d\n", (int)info.subsystem_type);
+		}
+	}
 
-// 			global_data_unlock(&global_data_subsystem_info->access_conf);
-// 		}
-// 	}
+	close(subsys_sub);
 
-// 	return NULL;
-// }
+	return NULL;
+}
 
 
 
@@ -736,16 +722,31 @@ enum BAT_CHEM {
  */
 float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
 
-PARAM_DEFINE_FLOAT(BAT_VOLT_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_VOLT_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
 
 float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
 {
 	float ret = 0;
-	// XXX do this properly
-	// XXX rebase on parameters
-	const float chemistry_voltage_empty[] = {3.2f};
-	const float chemistry_voltage_full[] = {4.05f};
+	static param_t bat_volt_empty;
+	static param_t bat_volt_full;
+	static bool initialized = false;
+	static unsigned int counter = 0;
+
+	if (!initialized) {
+		bat_volt_empty = param_find("BAT_V_EMPTY");
+		bat_volt_full = param_find("BAT_V_FULL");
+		initialized = true;
+	}
+
+	float chemistry_voltage_empty[1];
+	float chemistry_voltage_full[1];
+
+	if (counter % 100 == 0) {
+		param_get(bat_volt_empty, chemistry_voltage_empty+0);
+		param_get(bat_volt_full, chemistry_voltage_full+0);
+	}
+	counter++;
 
 	ret = (voltage - cells * chemistry_voltage_empty[chemistry]) / (cells * (chemistry_voltage_full[chemistry] - chemistry_voltage_empty[chemistry]));
 
@@ -817,9 +818,9 @@ int commander_thread_main(int argc, char *argv[])
 	/* welcome user */
 	printf("[commander] I am in command now!\n");
 
-	/* Pthreads */
+	/* pthreads for command and subsystem info handling */
 	pthread_t command_handling_thread;
-	// pthread_t subsystem_info_thread;
+	pthread_t subsystem_info_thread;
 
 	/* initialize */
 	if (led_init() != 0) {
@@ -853,31 +854,21 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_log_info(mavlink_fd, "[commander] system is running");
 
-	/* load EEPROM parameters */
-	if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
-		printf("[commander] Loaded EEPROM params in RAM\n");
-		mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM");
-
-	} else {
-		fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
-		mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
-	}
-
 	/* create pthreads */
 	pthread_attr_t command_handling_attr;
 	pthread_attr_init(&command_handling_attr);
 	pthread_attr_setstacksize(&command_handling_attr, 4096);
 	pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
 
-	// pthread_attr_t subsystem_info_attr;
-	// pthread_attr_init(&subsystem_info_attr);
-	// pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	// pthread_create(&subsystem_info_thread, &subsystem_info_attr, subsystem_info_loop, NULL);
+	pthread_attr_t subsystem_info_attr;
+	pthread_attr_init(&subsystem_info_attr);
+	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
+	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, NULL);
 
 	/* Start monitoring loop */
 	uint16_t counter = 0;
 	uint8_t flight_env;
-	// uint8_t fix_type;
+
 	/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
 	float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
 	bool battery_voltage_valid = true;
@@ -888,9 +879,6 @@ int commander_thread_main(int argc, char *argv[])
 	int16_t mode_switch_rc_value;
 	float bat_remain = 1.0f;
 
-//	bool arm_done = false;
-//	bool disarm_done = false;
-
 	uint16_t stick_off_counter = 0;
 	uint16_t stick_on_counter = 0;
 
@@ -919,7 +907,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* now initialized */
 	commander_initialized = true;
 
-	while (1) {
+	while (!thread_should_exit) {
 
 		/* Get current values */
 		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
@@ -930,8 +918,6 @@ int commander_thread_main(int argc, char *argv[])
 		battery_voltage_valid = sensors.battery_voltage_valid;
 		bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
 
-// 		flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
 		/* Slow but important 8 Hz checks */
 		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
 			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
@@ -1186,11 +1172,17 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* wait for threads to complete */
 	pthread_join(command_handling_thread, NULL);
-	// pthread_join(subsystem_info_thread, NULL);
+	pthread_join(subsystem_info_thread, NULL);
 
 	/* close fds */
 	led_deinit();
 	buzzer_deinit();
+	close(rc_sub);
+	close(gps_sub);
+	close(sensor_sub);
+
+	printf("[commander] exiting..\n");
+	fflush(stdout);
 
 	thread_running = false;
 
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index 42534a0a7..1d13b1c6a 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -248,7 +248,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
 			 * discrete low pass filter, cuts out the
 			 * high frequency noise that can drive the controller crazy
 			 */
-			float RC = 1.0 / (2.0f * M_PI * fCut);
+			float RC = 1.0f / (2.0f * M_PI_F * fCut);
 			derivative = lderiv +
 				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
 
@@ -288,17 +288,17 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
 	return output;
 }
 
-PARAM_DEFINE_FLOAT(FW_ATT_ROLL_P, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_ROLL_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_ROLL_D, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_ROLL_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_ROLL_LIM, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f);
+PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f);
+PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f);
 
-PARAM_DEFINE_FLOAT(FW_ATT_PITCH_P, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_PITCH_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_PITCH_D, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_PITCH_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f);
+PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f);
+PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f);
+PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f);
 
 /**
  * Load parameters from global storage.
@@ -308,21 +308,50 @@ PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f);
  * Fetches the current parameters from the global parameter storage and writes them
  * to the plane_data structure
  */
-static void get_parameters(plane_data_t * plane_data)
+static void get_parameters(plane_data_t * pdata)
 {
-	plane_data->Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
-	plane_data->Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I];
-	plane_data->Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D];
-	plane_data->Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P];
-	plane_data->Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I];
-	plane_data->Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D];
-	plane_data->intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU];
-	plane_data->intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU];
-	plane_data->airspeed =  global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED];
-	plane_data->wp_x =  global_data_parameter_storage->pm.param_values[PARAM_WPLON];
-	plane_data->wp_y =  global_data_parameter_storage->pm.param_values[PARAM_WPLAT];
-	plane_data->wp_z =  global_data_parameter_storage->pm.param_values[PARAM_WPALT];
-	plane_data->mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE];
+	static bool initialized = false;
+	static param_t att_p;
+	static param_t att_i;
+	static param_t att_d;
+	static param_t att_awu;
+	static param_t att_lim;
+
+	static param_t pos_p;
+	static param_t pos_i;
+	static param_t pos_d;
+	static param_t pos_awu;
+	static param_t pos_lim;
+
+	if (!initialized) {
+		att_p = param_find("FW_ATT_P");
+		att_i = param_find("FW_ATT_I");
+		att_d = param_find("FW_ATT_D");
+		att_awu = param_find("FW_ATT_AWU");
+		att_lim = param_find("FW_ATT_LIM");
+
+		pos_p = param_find("FW_POS_P");
+		pos_i = param_find("FW_POS_I");
+		pos_d = param_find("FW_POS_D");
+		pos_awu = param_find("FW_POS_AWU");
+		pos_lim = param_find("FW_POS_LIM");
+
+		initialized = true;
+	}
+
+	param_get(att_p, &(pdata->Kp_att));
+	param_get(att_i, &(pdata->Ki_att));
+	param_get(att_d, &(pdata->Kd_att));
+	param_get(pos_p, &(pdata->Kp_pos));
+	param_get(pos_i, &(pdata->Ki_pos));
+	param_get(pos_d, &(pdata->Kd_pos));
+	param_get(att_awu, &(pdata->intmax_att));
+	param_get(pos_awu, &(pdata->intmax_pos));
+	pdata->airspeed = 10;
+	pdata->wp_x =  48;
+	pdata->wp_y =  8;
+	pdata->wp_z =  100;
+	pdata->mode = 1;
 }
 
 /**
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 1b23f6751..4f3a8295d 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -75,9 +75,15 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <systemlib/param/param.h>
+
 #include "waypoints.h"
 #include "mavlink_log.h"
 
+/* define MAVLink specific parameters */
+PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_GENERIC);
 
 __EXPORT int mavlink_main(int argc, char *argv[]);
 int mavlink_thread_main(int argc, char *argv[]);
@@ -90,8 +96,7 @@ static int mavlink_task;
 static bool mavlink_link_termination_allowed = false;
 static bool mavlink_exit_requested = false;
 
-static int system_type = MAV_TYPE_FIXED_WING;
-mavlink_system_t mavlink_system = {100, 50, 0, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
+mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
 static uint8_t chan = MAVLINK_COMM_0;
 static mavlink_status_t status;
 
@@ -531,7 +536,7 @@ static void *uorb_receiveloop(void *arg)
 	struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg;
 
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink uORB", getpid());
+	prctl(PR_SET_NAME, "mavlink orb rcv", getpid());
 
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
@@ -787,7 +792,7 @@ static void *uorb_receiveloop(void *arg)
 				get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
 
 				/* send heartbeat */
-				mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state);
+				mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
 			}
 
 			/* --- RC CHANNELS --- */
@@ -1080,7 +1085,7 @@ void handleMessage(mavlink_message_t *msg)
 			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 			//check for MAVLINK terminate command
 			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
-				/* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app
+				/* This is the link shutdown command, terminate mavlink */
 				printf("[mavlink] Terminating .. \n");
 				fflush(stdout);
 				usleep(50000);
@@ -1250,63 +1255,6 @@ void handleMessage(mavlink_message_t *msg)
 			hil_attitude.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
 		}
-
-		// 	if (msg->msgid == MAVLINK_MSG_ID_ATTITUDE) {
-		// 		mavlink_attitude_t att;
-		// 		mavlink_msg_attitude_decode(msg, &att);
-		// 		float RAD2DEG = 57.3f;
-
-		// //		printf("\n\n\n ATTITUDE \n\n\n %i \n", (int)(1000*att.rollspeed));
-
-		// 		global_data_lock(&global_data_attitude->access_conf);
-		// 		global_data_attitude->roll = RAD2DEG * att.roll;
-		// 		global_data_attitude->pitch = RAD2DEG * att.pitch;
-		// 		global_data_attitude->yaw = RAD2DEG * att.yaw;
-		// 		global_data_attitude->rollspeed = att.rollspeed;
-		// 		global_data_attitude->pitchspeed = att.pitchspeed;
-		// 		global_data_attitude->yawspeed = att.yawspeed;
-
-		// 		global_data_attitude->counter++;
-		// 		global_data_attitude->timestamp = hrt_absolute_time();
-		// 		global_data_unlock(&global_data_attitude->access_conf);
-		// 		global_data_broadcast(&global_data_attitude->access_conf);
-		// 	}
-
-		// 	if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
-		// 		mavlink_raw_imu_t imu;
-		// 		mavlink_msg_raw_imu_decode(msg, &imu);
-
-		// //		printf("\n\n\n RAW_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro),
-		// //						(int)(1000*imu.ygyro), (int)(1000*imu.zgyro));
-
-		// 		global_data_lock(&global_data_attitude->access_conf);
-		// 		global_data_attitude->rollspeed = 1000 * imu.xgyro;
-		// 		global_data_attitude->pitchspeed = 1000 * imu.ygyro;
-		// 		global_data_attitude->yawspeed = 1000 * imu.zgyro;
-
-		// 		global_data_attitude->counter++;
-		// 		global_data_attitude->timestamp = hrt_absolute_time();
-		// 		global_data_unlock(&global_data_attitude->access_conf);
-		// 		global_data_broadcast(&global_data_attitude->access_conf);
-		// 	}
-
-		// 	if (msg->msgid == MAVLINK_MSG_ID_SCALED_IMU) {
-		// 		mavlink_raw_imu_t imu;
-		// 		mavlink_msg_raw_imu_decode(msg, &imu);
-
-		// //		printf("\n\n\n SCALED_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro),
-		// //				(int)(1000*imu.ygyro), (int)(1000*imu.zgyro));
-
-		// 		global_data_lock(&global_data_attitude->access_conf);
-		// 		global_data_attitude->rollspeed = 1000 * imu.xgyro;
-		// 		global_data_attitude->pitchspeed = 1000 * imu.ygyro;
-		// 		global_data_attitude->yawspeed = 1000 * imu.zgyro;
-
-		// 		global_data_attitude->counter++;
-		// 		global_data_attitude->timestamp = hrt_absolute_time();
-		// 		global_data_unlock(&global_data_attitude->access_conf);
-		// 		global_data_broadcast(&global_data_attitude->access_conf);
-		// 	}
 	}
 }
 
@@ -1483,6 +1431,11 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* Flush UART */
 	fflush(stdout);
 
+	/* Initialize system properties */
+	param_t param_system_id = param_find("MAV_SYS_ID");
+	param_t param_component_id = param_find("MAV_COMP_ID");
+	param_t param_system_type = param_find("MAV_TYPE");
+
 	/* topics to subscribe globally */
 	/* subscribe to ORB for global position */
 	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -1507,7 +1460,8 @@ int mavlink_thread_main(int argc, char *argv[])
 	mavlink_wpm_init(wpm);
 
 	uint16_t counter = 0;
-	int lowspeed_counter = 0;
+	/* arm counter to go off immediately */
+	int lowspeed_counter = 10;
 
 	/* make sure all threads have registered their subscriptions */
 	while (!mavlink_subs.initialized) {
@@ -1565,21 +1519,34 @@ int mavlink_thread_main(int argc, char *argv[])
 		orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
 		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 
-		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		/* 1 Hz */
+		if (lowspeed_counter == 10) {
+			/* update system and component id */
+			int32_t system_id;
+			param_get(param_system_id, &system_id);
+			if (system_id > 0 && system_id < 255) {
+				mavlink_system.sysid = system_id;
+			}
 
-		// sleep
-		usleep(50000);
+			int32_t component_id;
+			param_get(param_component_id, &component_id);
+			if (component_id > 0 && component_id < 255) {
+				mavlink_system.compid = component_id;
+			}
+			
+			int32_t system_type;
+			param_get(param_system_type, &system_type);
+			if (system_type >= 0 && system_type < MAV_AUTOPILOT_ENUM_END) {
+				mavlink_system.type = system_type;
+			}
 
-		// 1 Hz
-		if (lowspeed_counter == 10) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_mode = 0;
 			get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled,
@@ -1595,18 +1562,33 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			lowspeed_counter = 0;
 		}
-
 		lowspeed_counter++;
 
+		/* sleep quarter the time */
+		usleep(25000);
+
+		/* check if waypoint has been reached against the last positions */
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+
+		/* sleep quarter the time */
+		usleep(25000);
+
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
-		usleep(50000);
+		/* sleep quarter the time */
+		usleep(25000);
 		mavlink_pm_queued_send();
 
+		/* sleep 10 ms */
+		usleep(10000);
+
 		/* send one string at 10 Hz */
 		mavlink_missionlib_send_gcs_string(mavlink_message_string);
 		mavlink_message_string[0] = '\0';
 		counter++;
+
+		/* sleep 15 ms */
+		usleep(15000);
 	}
 
 	/* wait for threads to complete */
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c
index 41ef47918..1ffe9f7bd 100644
--- a/apps/px4/attitude_estimator_bm/attitude_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_bm.c
@@ -207,10 +207,6 @@ void attitude_blackmagic_init(void)
 
 void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro)
 {
-	//Transform accelerometer used in all directions
-	//	float_vect3 acc_nav;
-	//body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
-
 	// Kalman Filter
 
 	//Calculate new linearized A matrix
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index b1a8de7bd..1433d1113 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -123,142 +123,6 @@ void kill_task(FAR _TCB *tcb, FAR void *arg)
 	kill(tcb->pid, SIGUSR1);
 }
 
-union param_union {
-	float f;
-	char c[4];
-};
-
-int store_params_in_eeprom(struct global_data_parameter_storage_t *params)
-{
-	int ret = ERROR;
-	int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
-	int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes
-	int write_res;
-
-	if (fd < 0) {
-		fprintf(stderr, "onboard eeprom: open fail\n");
-		ret = ERROR;
-
-	} else if (lseek_res < 0) {
-		fprintf(stderr, "onboard eeprom: set offet fail\n");
-		ret = ERROR;
-
-	} else {
-		/*Write start magic byte */
-		uint8_t mb = EEPROM_PARAM_MAGIC_BYTE;
-		write_res = write(fd, &mb, sizeof(mb));
-
-		if (write_res != sizeof(mb)) {
-			ret = ERROR;
-
-		} else {
-			for (int i = 0; i < PARAM_MAX_COUNT; i++) {
-
-				union param_union p;
-				p.f = params->pm.param_values[i];
-				write_res = write(fd, p.c, sizeof(p.f));
-
-				if (write_res != sizeof(p.f)) return ERROR;
-			}
-
-			/*Write end magic byte */
-			write_res = write(fd, &mb, sizeof(mb));
-
-			if (write_res != sizeof(mb)) {
-				ret = ERROR;
-
-			} else {
-				ret = OK;
-			}
-		}
-
-	}
-
-	close(fd);
-
-	return ret;
-}
-
-int get_params_from_eeprom(struct global_data_parameter_storage_t *params)
-{
-	int ret = ERROR;
-	uint8_t magic_byte = 0;
-	int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
-	int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET);  //don't touch first 64 bytes
-
-	if (fd < 0) {
-		fprintf(stderr, "onboard eeprom: open fail\n");
-		ret = ERROR;
-
-	} else if (lseek_res < 0) {
-		fprintf(stderr, "onboard eeprom: set offet fail\n");
-		ret = ERROR;
-
-	} else {
-		/*Get start magic byte */
-		magic_byte = 0;
-		int read_res = read(fd, &magic_byte, sizeof(uint8_t));
-
-		if (read_res != sizeof(uint8_t)) {
-			ret = ERROR;
-
-		} else {
-			if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) {
-				ret = ERROR;
-				fprintf(stderr, "onboard eeprom: parameters: start magic byte wrong\n");
-
-			} else {
-				/*get end magic byte */
-				lseek_res = lseek(fd, EEPROM_OFFSET + 1 + params->pm.size * sizeof(float), SEEK_SET); // jump to 2nd magic byte
-
-				if (lseek_res == OK) {
-					/*Get end magic */
-					read_res = read(fd, &magic_byte, sizeof(uint8_t));
-
-					if (read_res != sizeof(uint8_t)) {
-						ret = ERROR;
-
-					} else {
-						if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) {
-							ret = ERROR;
-							printf("onboard eeprom: parameters: end magic byte wrong\n");
-
-						} else {
-							lseek_res = lseek(fd, EEPROM_OFFSET + 1, SEEK_SET);
-
-							/* read data */
-							if (lseek_res == OK) {
-
-								for (int i = 0; i < PARAM_MAX_COUNT; i++) {
-									union param_union p;
-									read_res = read(fd, p.c, sizeof(p.f));
-									params->pm.param_values[i] = p.f;
-									if (read_res != sizeof(p.f)) return ERROR;
-								}
-
-								ret = OK;
-
-							} else {
-								/* lseek #2 failed */
-								ret = ERROR;
-							}
-						}
-					}
-
-				} else {
-					/* lseek #1 failed */
-					ret = ERROR;
-				}
-			}
-		}
-
-	}
-
-	close(fd);
-
-	return ret;
-}
-
 #define PX4_BOARD_ID_FMU (5)
 
 int fmu_get_board_info(struct fmu_board_info_s *info)
@@ -268,23 +132,20 @@ int fmu_get_board_info(struct fmu_board_info_s *info)
 	int statres;
 
 	/* Copy version-specific fields */
-	statres = stat("/dev/bma280", &sb);
+	statres = stat("/dev/bma180", &sb);
 
 	if (statres == OK) {
-		/* BMA280 indicates a v1.7+ board */
-		strcpy(info->board_name, "FMU v1.7");
-		info->board_version = 17;
-
-	} else {
-		statres = stat("/dev/bma180", &sb);
-
-		if (statres == OK) {
 			/* BMA180 indicates a v1.5-v1.6 board */
 			strcpy(info->board_name, "FMU v1.6");
 			info->board_version = 16;
-
+	} else {
+		statres = stat("/dev/accel", &sb);
+		if (statres == OK) {
+			/* MPU-6000 indicates a v1.7+ board */
+			strcpy(info->board_name, "FMU v1.7");
+			info->board_version = 17;
 		} else {
-			/* If no BMA pressure sensor is present, it is a v1.3 board */
+			/* If no BMA and no MPU is present, it is a v1.3 board */
 			strcpy(info->board_name, "FMU v1.3");
 			info->board_version = 13;
 		}
diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h
index eec43a5b4..2824cbec9 100644
--- a/apps/systemlib/systemlib.h
+++ b/apps/systemlib/systemlib.h
@@ -50,12 +50,6 @@ __EXPORT int reboot(void);
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
-struct global_data_parameter_storage_t;
-
-__EXPORT int store_params_in_eeprom(struct global_data_parameter_storage_t *params);
-
-__EXPORT int get_params_from_eeprom(struct global_data_parameter_storage_t *params);
-
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,
 	MULT_1_US2_FLOW,
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index d6c00f996..c60a93fcb 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -108,6 +108,9 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);
 
+#include "topics/subsystem_info.h"
+ORB_DEFINE(subsystem_info, struct subsystem_info_s);
+
 #include "topics/actuator_controls.h"
 ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h
index 5bd2adeec..28cc5d7c4 100644
--- a/apps/uORB/topics/rc_channels.h
+++ b/apps/uORB/topics/rc_channels.h
@@ -62,11 +62,6 @@ enum RC_CHANNELS_STATUS
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
  * the channel array chan[].
- * Ex. To read out the scaled Pitch value:
- * pitch = global_data_rc_channels->chan[PITCH].scale;
- * The override is on channel 8, since we don't usually have a 12 channel RC
- * and channel 5/6 (GRAUPNER/FUTABA) are mapped to the second ROLL servo, which
- * can only be disabled on more advanced RC sets.
  */
 enum RC_CHANNELS_FUNCTION
 {
@@ -98,7 +93,7 @@ struct rc_channels_s {
     uint16_t override;
     enum RC_CHANNELS_STATUS status;   /**< status of the channel */
   } chan[RC_CHANNELS_FUNCTION_MAX];
-  uint8_t chan_count;                    /**< maximum number of valid channels */
+  uint8_t chan_count;                 /**< maximum number of valid channels */
 
   /*String array to store the names of the functions*/
   const char function_name[RC_CHANNELS_FUNCTION_MAX][20];
diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h
new file mode 100644
index 000000000..89b397478
--- /dev/null
+++ b/apps/uORB/topics/subsystem_info.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *
+ * 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 subsystem_info.h
+ * Definition of the subsystem info topic.
+ */
+
+#ifndef TOPIC_SUBSYSTEM_INFO_H_
+#define TOPIC_SUBSYSTEM_INFO_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+enum SUBSYSTEM_TYPE
+{
+	SUBSYSTEM_TYPE_GYRO = 1,
+	SUBSYSTEM_TYPE_ACC = 2,
+	SUBSYSTEM_TYPE_MAG = 4,
+	SUBSYSTEM_TYPE_ABSPRESSURE = 8,
+	SUBSYSTEM_TYPE_DIFFPRESSURE = 16,
+	SUBSYSTEM_TYPE_GPS = 32,
+	SUBSYSTEM_TYPE_OPTICALFLOW = 64,
+	SUBSYSTEM_TYPE_CVPOSITION = 128,
+	SUBSYSTEM_TYPE_LASERPOSITION = 256,
+	SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512,
+	SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024,
+	SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048,
+	SUBSYSTEM_TYPE_YAWPOSITION = 4096,
+	SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384,
+	SUBSYSTEM_TYPE_POSITIONCONTROL = 32768,
+	SUBSYSTEM_TYPE_MOTORCONTROL = 65536
+};
+
+/**
+ * State of individual sub systems
+ */
+struct subsystem_info_s {
+	bool present;
+	bool enabled;
+	bool ok;
+
+	enum SUBSYSTEM_TYPE subsystem_type;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(subsystem_info);
+
+#endif /* TOPIC_SUBSYSTEM_INFO_H_ */
+