Various fixes for params interface

5 files changed, 53 insertions, 35 deletions

diff --git a/apps/commander/Makefile b/apps/commander/Makefile
index a1149eb8d..d12697274 100644
--- a/apps/commander/Makefile
+++ b/apps/commander/Makefile
@@ -36,8 +36,8 @@
 #
 
 APPNAME		 = commander
-PRIORITY	 = SCHED_PRIORITY_MAX - 10
-STACKSIZE	 = 3072
+PRIORITY	 = SCHED_PRIORITY_MAX - 30
+STACKSIZE	 = 2048
 
 INCLUDES	= $(TOPDIR)/../mavlink/include/mavlink
 
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 21bf91c61..34dfc22b5 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -532,8 +532,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
+					mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration");
 					do_gyro_calibration(status_pub, &current_status);
+					mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
@@ -549,12 +550,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
+					mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration");
 					do_mag_calibration(status_pub, &current_status);
+					mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
 				} else {
-					mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
+					mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration");
 					result = MAV_RESULT_DENIED;
 				}
 				handled = true;
@@ -562,8 +564,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* none found */
 			if (!handled) {
-				fprintf(stderr, "[commander] refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request");
+				//fprintf(stderr, "[commander] refusing unsupported calibration request\n");
+				mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request");
 				result = MAV_RESULT_UNSUPPORTED;
 			}
 		}
@@ -576,12 +578,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			if (((int)cmd->param1) == 0)	{
 
 				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");
+					//printf("[commander] Loaded EEPROM params in RAM\n");
+					mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM");
 					result = MAV_RESULT_ACCEPTED;
 
 				} else {
-					fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
+					//fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
 					mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
 					result = MAV_RESULT_FAILED;
 				}
@@ -591,18 +593,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			} else if (((int)cmd->param1) == 1)	{
 
 				if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
-					printf("[commander] RAM params written to EEPROM\n");
+					//printf("[commander] RAM params written to EEPROM\n");
 					mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
 					result = MAV_RESULT_ACCEPTED;
 
 				} else {
-					fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
+					//fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
 					mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
 					result = MAV_RESULT_FAILED;
 				}
 
 			} else {
-				fprintf(stderr, "[commander] refusing unsupported storage request\n");
+				//fprintf(stderr, "[commander] refusing unsupported storage request\n");
 				mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
 				result = MAV_RESULT_UNSUPPORTED;
 			}
@@ -622,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0) {
 		/* send acknowledge command */
-		mavlink_message_t msg;
-		mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
 	}
 
 }
@@ -788,6 +788,7 @@ int commander_main(int argc, char *argv[])
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+	/* publish current state machine */
 	state_machine_publish(stat_pub, &current_status, mavlink_fd);
 
 	if (stat_pub < 0) {
@@ -874,9 +875,9 @@ int commander_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]);
+// 		flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
 
-		/* Slow but important 5 Hz checks */
+		/* 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 */
 			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
@@ -994,14 +995,14 @@ int commander_main(int argc, char *argv[])
 //
 //
 //		if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
-		update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
+		//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
 		/* end: check gps */
 
 		/* Check battery voltage */
 		/* write to sys_status */
 		current_status.voltage_battery = battery_voltage;
 
-		/* if battery voltage is getting lower, warn using buzzer, etc.  */
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
 
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
@@ -1086,8 +1087,8 @@ int commander_main(int argc, char *argv[])
 
 		} else {
 			static uint64_t last_print_time = 0;
-			/* print error message for first RC glitch and then every 2 s / 2000 ms) */
-			if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) {
+			/* print error message for first RC glitch and then every 5 s / 5000 ms) */
+			if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
 				mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
 				last_print_time = hrt_absolute_time();
 			}
@@ -1105,7 +1106,7 @@ int commander_main(int argc, char *argv[])
 
 		current_status.counter++;
 		current_status.timestamp = hrt_absolute_time();
-		if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
 
 		/* If full run came back clean, transition to standby */
 		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
@@ -1115,8 +1116,12 @@ int commander_main(int argc, char *argv[])
 			do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
 		}
 
+		/* publish at least with 1 Hz */
+		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+		}
+
 		/* Store old modes to detect and act on state transitions */
-		// vehicle_state_previous = current_status.state_machine;
 		voltage_previous = current_status.voltage_battery;
 
 		fflush(stdout);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index dc8b2a2c8..8e0fe629b 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1270,13 +1270,13 @@ int mavlink_main(int argc, char *argv[])
 
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
+	pthread_attr_setstacksize(&receiveloop_attr, 4096);
 	pthread_create(&receive_thread, &receiveloop_attr, receiveloop, NULL);
 
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
-	/* Set stack size, needs more than 2048 bytes */
-	pthread_attr_setstacksize(&uorb_attr, 5096);
+	/* Set stack size, needs more than 5000 bytes */
+	pthread_attr_setstacksize(&uorb_attr, 7000);
 	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
 
 	/* initialize waypoint manager */
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 609cf5d34..b1a8de7bd 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -123,6 +123,11 @@ 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;
@@ -147,10 +152,13 @@ int store_params_in_eeprom(struct global_data_parameter_storage_t *params)
 			ret = ERROR;
 
 		} else {
-			for (int i = 0; i < params->pm.size; i++) {
-				write_res = write(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i]));
+			for (int i = 0; i < PARAM_MAX_COUNT; i++) {
 
-				if (write_res != sizeof(params->pm.param_values[i])) return ERROR;
+				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 */
@@ -221,10 +229,11 @@ int get_params_from_eeprom(struct global_data_parameter_storage_t *params)
 							/* read data */
 							if (lseek_res == OK) {
 
-								for (int i = 0; i < params->pm.size; i++) {
-									read_res = read(fd, params->pm.param_values + i,  sizeof(params->pm.param_values[i]));
-
-									if (read_res != sizeof(params->pm.param_values[i])) return ERROR;
+								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;
diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h
index 54ae63cb9..aa4c1bb4f 100644
--- a/apps/uORB/parameter_storage.h
+++ b/apps/uORB/parameter_storage.h
@@ -112,25 +112,28 @@ enum PARAM {
 	PARAM_MAX_COUNT        ///< LEAVE THIS IN PLACE AS LAST ELEMENT!
 };
 
+#pragma pack(push, 1)
 struct px4_parameter_storage_t {
 	float param_values[PARAM_MAX_COUNT];      ///< Parameter values
-	const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
 	bool param_needs_write[PARAM_MAX_COUNT];
+	const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
 	uint16_t next_param;
 	uint16_t size;
 };
+#pragma pack(pop)
 
 
 #define PX4_FLIGHT_ENVIRONMENT_INDOOR 0
 #define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1
 #define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment
 
+#pragma pack(push, 1)
 struct global_data_parameter_storage_t {
 
 	/* use of a counter and timestamp recommended (but not necessary) */
 
 //	uint16_t counter; //incremented by the writing thread everytime new data is stored
-//	uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+	uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
 
 	/* Actual data, this is specific to the type of data which is stored in this struct */
 
@@ -142,6 +145,7 @@ struct global_data_parameter_storage_t {
 	//*****END: Add your variables here *****
 
 };
+#pragma pack(pop)
 
 __attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line!