Completed calibration state machine, calibration state now propagating to sensor, scale calibration soon

5 files changed, 105 insertions, 54 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 2a1ed7532..3236c2313 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -103,8 +103,8 @@ static int stat_pub;
 static uint16_t nofix_counter = 0;
 static uint16_t gotfix_counter = 0;
 
-static void do_gyro_calibration(void);
-static void do_mag_calibration(void);
+static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
+static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status);
 static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
 
 /* pthread loops */
@@ -229,13 +229,17 @@ void cal_bsort(int16_t a[], int n)
 	}
 }
 
-void do_mag_calibration(void)
+void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
 {
+	/* set to mag calibration mode */
+	current_status->preflight_mag_calibration = true;
+	state_machine_publish(status_pub, current_status);
+
 	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	const uint64_t calibration_interval_us = 5 * 1000000;
+	const uint64_t calibration_interval_us = 10 * 1000000;
 	unsigned int calibration_counter = 0;
 
 	const int peak_samples = 2000;
@@ -264,8 +268,7 @@ void do_mag_calibration(void)
 	}
 	
 	uint64_t calibration_start = hrt_absolute_time();
-	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us
-		&& calibration_counter < peak_samples) {
+	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
 
 		/* wait blocking for new data */
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@@ -305,6 +308,10 @@ void do_mag_calibration(void)
 		}
 	}
 
+	/* disable calibration mode */
+	current_status->preflight_mag_calibration = false;
+	state_machine_publish(status_pub, current_status);
+
 	/* sort values */
 	cal_bsort(mag_minima[0], peak_samples);
 	cal_bsort(mag_minima[1], peak_samples);
@@ -389,7 +396,7 @@ void do_mag_calibration(void)
 	close(sub_sensor_combined);
 }
 
-void do_gyro_calibration(void)
+void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status)
 {
 	const int calibration_count = 3000;
 
@@ -446,8 +453,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 	/* request to set different system mode */
 	switch (cmd->command) {
-
-
 		case MAV_CMD_DO_SET_MODE:
 		{
 			update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1);
@@ -509,17 +514,36 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* gyro calibration */
 			if ((int)(cmd->param1) == 1) {
-				mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
-				do_gyro_calibration();
-				result = MAV_RESULT_ACCEPTED;
+				/* transition to calibration state */
+				do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
+
+				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+					mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
+					do_gyro_calibration(status_pub, &current_status);
+					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
+					result = MAV_RESULT_ACCEPTED;
+				} else {
+					mavlink_log_info(mavlink_fd, "[commander] REJECTING gyro calibration");
+					result = MAV_RESULT_DENIED;
+				}
 				handled = true;
 			}
 
 			/* magnetometer calibration */
 			if ((int)(cmd->param2) == 1) {
-				mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
-				do_mag_calibration();
-				result = MAV_RESULT_ACCEPTED;
+				/* transition to calibration state */
+				do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
+
+				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+					mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
+					do_mag_calibration(status_pub, &current_status);
+					do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
+					result = MAV_RESULT_ACCEPTED;
+				} else {
+					mavlink_log_info(mavlink_fd, "[commander] REJECTING mag calibration");
+					result = MAV_RESULT_DENIED;
+				}
+				handled = true;
 			}
 
 			/* none found */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 2171ccedf..91eea2117 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -4,7 +4,6 @@
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           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:
@@ -34,7 +33,10 @@
  *
  ****************************************************************************/
 
-/* @file State machine helper functions implementations */
+/**
+ * @file state_machine_helper.c
+ * State machine helper functions implementations
+ */
 
 #include <stdio.h>
 #include "state_machine_helper.h"
@@ -64,6 +66,8 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 {
 	int invalid_state = false;
 
+	commander_state_machine_t old_state = current_status->state_machine;
+
 	switch (new_state) {
 	case SYSTEM_STATE_MISSION_ABORT: {
 			/* Indoor or outdoor */
@@ -103,6 +107,12 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 
 	case SYSTEM_STATE_PREFLIGHT:
 		//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO);
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY
+		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			invalid_state = false;
+		} else {
+			invalid_state = true;
+		}
 		break;
 
 	case SYSTEM_STATE_REBOOT:
@@ -143,16 +153,20 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 		break;
 	}
 
-	if (invalid_state == false) {
-		//publish the new state
+	if (invalid_state == false || old_state != new_state) {
 		current_status->state_machine = new_state;
-		current_status->counter++;
-		current_status->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-		printf("[commander] new state: %s\n", system_state_txt[new_state]);
+		state_machine_publish(status_pub, current_status);
 	}
 }
 
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) {
+	/* publish the new state */
+	current_status->counter++;
+	current_status->timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
+}
+
 
 /*
  * Private functions, update the state machine
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 0abc6349c..540182e4f 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -4,7 +4,6 @@
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           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:
@@ -34,7 +33,10 @@
  *
  ****************************************************************************/
 
-/* @file State machine helper functions definitions */
+/**
+ * @file state_machine_helper.h
+ * State machine helper functions definitions
+ */
 
 #ifndef STATE_MACHINE_HELPER_H_
 #define STATE_MACHINE_HELPER_H_
@@ -47,14 +49,13 @@
 #include <v1.0/common/mavlink.h>
 
 
-/*
- * @brief switch to new state with no checking *
+/**
+ * Switch to new state with no checking.
  *
  * do_state_update: this is the functions that all other functions have to call in order to update the state.
  * the function does not question the state change, this must be done before
  * The function performs actions that are connected with the new state (buzzer, reboot, ...)
  *
- *
  */
 void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state);
 
@@ -69,42 +70,45 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
 // void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
 
 
-/*
- * @brief handle state machine if got position fix
+/**
+ * Handle state machine if got position fix
  */
 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if position fix lost
+/**
+ * Handle state machine if position fix lost
  */
 void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if user wants to arm
+/**
+ * Handle state machine if user wants to arm
  */
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if user wants to disarm
+/**
+ * Handle state machine if user wants to disarm
  */
 void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if mode switch is manual
+/**
+ * Handle state machine if mode switch is manual
  */
 void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if mode switch is stabilized
+/**
+ * Handle state machine if mode switch is stabilized
  */
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status);
 
-/*
- * @brief handle state machine if mode switch is auto
+/**
+ * Handle state machine if mode switch is auto
  */
 void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status);
 
-
+/**
+ * Publish current state information
+ */
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status);
 
 
 /*
@@ -113,13 +117,13 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
  *
  */
 
-/*
- * @brief handles *incoming request* to switch to a specific state, if state change is successful returns 0
+/**
+ * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
  */
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode);
 
-/*
- * @brief handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
+/**
+ * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
  */
 uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode);
 
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index e3c8e5fa1..764c9ffed 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -269,8 +269,13 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t
 	//TODO: Make this correct
 	switch (c_status->state_machine) {
 	case SYSTEM_STATE_PREFLIGHT:
-		*mavlink_state = MAV_STATE_UNINIT;
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+		if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) {
+			*mavlink_state = MAV_STATE_CALIBRATING;
+			*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+		} else {
+			*mavlink_state = MAV_STATE_UNINIT;
+			*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+		}
 		break;
 
 	case SYSTEM_STATE_STANDBY:
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 7e8eac453..45e7687fd 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -316,6 +316,8 @@ int sensors_main(int argc, char *argv[])
 	int16_t acc_offset[3] = {0, 0, 0};
 	int16_t	gyro_offset[3] = {0, 0, 0};
 
+	bool mag_calibration_enabled = false;
+
 	#pragma pack(push,1)
 	struct adc_msg4_s {
 		uint8_t      am_channel1;	/**< The 8-bit ADC Channel 1 */
@@ -345,9 +347,7 @@ int sensors_main(int argc, char *argv[])
 #endif
 	/* initialize to 100 to execute immediately */
 	int paramcounter = 100;
-
 	int excessive_readout_time_counter = 0;
-
 	int read_loop_counter = 0;
 
 	/* Empty sensor buffers, avoid junk values */
@@ -565,10 +565,14 @@ int sensors_main(int argc, char *argv[])
 			if (magcounter == 4) { /* 120 Hz */
 				uint64_t start_mag = hrt_absolute_time();
 				/* start calibration mode if requested */
-				if (raw.magnetometer_mode == MAGNETOMETER_MODE_NORMAL && vstatus.preflight_mag_calibration) {
+				if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) {
 					ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0);
-				} else if (raw.magnetometer_mode != MAGNETOMETER_MODE_NORMAL && !vstatus.preflight_mag_calibration) {
+					printf("[sensors] enabling mag calibration mode\n");
+					mag_calibration_enabled = true;
+				} else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) {
 					ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0);
+					printf("[sensors] disabling mag calibration mode\n");
+					mag_calibration_enabled = false;
 				}
 
 				ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));