ESC calibration re-work

- Uses standard QGC cal mavlink messaging
- Timeout if no battery connect
- Better error handling and cleanup

src/modules/commander/calibration_messages.h

@@ -52,6 +52,7 @@
 #define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
 #define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
 #define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
+#define CAL_QGC_WARNING_MSG			"[cal] calibration warning: %s"
 #define CAL_QGC_CANCELLED_MSG			"[cal] calibration cancelled"
 #define CAL_QGC_PROGRESS_MSG			"[cal] progress <%u>"
 #define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"

src/modules/commander/esc_calibration.cpp

@@ -40,6 +40,8 @@
  */
 
 #include "esc_calibration.h"
+#include "calibration_messages.h"
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -63,91 +65,113 @@
 #endif
 static const int ERROR = -1;
 
-int check_if_batt_disconnected(int mavlink_fd) {
+int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
-	struct battery_status_s battery;
+{
-	memset(&battery,0,sizeof(battery));
+	int	return_code = OK;
-	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+	int	fd = -1;
-	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+	struct	battery_status_s battery;
-
+	int	batt_sub = -1;
-	if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
+	bool	batt_updated = false;
-		mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!");
+	bool	batt_connected = false;
-		return ERROR;
+	hrt_abstime start_time;
+	
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
+	
+	batt_sub = orb_subscribe(ORB_ID(battery_status));
+	if (batt_sub < 0) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery");
+		goto Error;
 	}
-	return OK;
-}
 
+	// Make sure battery is disconnected
+	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+	if (battery.voltage_filtered_v > 3.0f) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
+		goto Error;
+	}
+	
+	armed->in_esc_calibration_mode = true;
+	
+	fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
 
-int do_esc_calibration(int mavlink_fd) {
+	if (fd < 0) {
-
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
-	int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
+		goto Error;
-	int ret;
-
-	if(fd < 0) {
-		err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
 	}
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
-	ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
-	if (ret != OK)
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
-		err(1, "PWM_SERVO_SET_ARM_OK");
+		goto Error;
+	}
+	
 	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
+	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
-	if (ret != OK)
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system");
-		err(1, "PWM_SERVO_ARM");
+		goto Error;
+	}
+	
 	/* tell IO to switch off safety without using the safety switch */
-	ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
-	if(ret!=0) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off");
-		err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
+		goto Error;
 	}
 
-	mavlink_log_info(mavlink_fd,"Please connect battery now");
+	mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
+	
+	start_time = hrt_absolute_time();
 
-	struct battery_status_s battery;
+	while (true) {
-	memset(&battery,0,sizeof(battery));
+		if (hrt_absolute_time() - start_time > 5000000) {
-	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+			if (!batt_connected) {
-	orb_copy(ORB_ID(vehicle_command),batt_sub, &battery);
+				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
-	bool updated = false;
+				goto Error;
-
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd);
-
-	/* wait for one of the following events:
-		1) user has pressed the button in QGroundControl
-		2) timeout of 5 seconds is reached
-	*/
-	hrt_abstime start_time = hrt_absolute_time();
-
-	while(true) {
-		orb_check(batt_sub,&updated);
-		if(updated) {
-			orb_copy(ORB_ID(battery_status), batt_sub, &battery);
-		}
-		// user has connected battery
-		if(battery.voltage_filtered_v > 3.0f) {
-			orb_check(cmd_sub,&updated);
-			if(updated) {
-				orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-			}
-			if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
-				break;
-			} else if (hrt_absolute_time() - start_time > 5000000) {
-				// waited for 5 seconds, switch to low pwm
-				break;
 			}
+			
+			// 5 seconds at high pwm
+			break;
 		}
-		else {
+		
-			start_time = hrt_absolute_time();
+		if (!batt_connected) {
+			orb_check(batt_sub, &batt_updated);
+			if (batt_updated) {
+				orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+				if (battery.voltage_filtered_v > 3.0f) {
+					// Battery connected, wait for 5 seconds at high pwm
+					batt_connected = true;
+					start_time = hrt_absolute_time();
+					mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
+				}
+			}
 		}
 		usleep(50000);
 	}
 
-	/* disarm */
+Out:
-	ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
+	if (batt_sub != -1) {
-	if (ret != OK)
+		orb_unsubscribe(batt_sub);
-		err(1, "PWM_SERVO_DISARM");
+	}
-
+	if (fd != -1) {
-	mavlink_log_info(mavlink_fd,"ESC calibration finished");
+		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
-	return OK;
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off");
- }
+		}
+		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed");
+		}
+		if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
+		}
+		px4_close(fd);
+	}
+	armed->in_esc_calibration_mode = false;
+	
+	if (return_code == OK) {
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc");
+	}
+	
+	return return_code;
+	
+Error:
+	return_code = ERROR;
+	goto Out;
+}

src/modules/commander/esc_calibration.h

@@ -41,7 +41,9 @@
 
 #ifndef ESC_CALIBRATION_H_
 #define ESC_CALIBRATION_H_
-int check_if_batt_disconnected(int mavlink_fd);
+
-int do_esc_calibration(int mavlink_fd);
+#include <uORB/topics/actuator_armed.h>
+
+int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed);
 
 #endif