From 8b629454de069c835ad086b78e498f955c5193fd Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Fri, 20 Apr 2018 15:06:56 +0200
Subject: [PATCH] esc_calibration: increase safety and initialise all data - do
 not do calibration if not very sure that battery is not connected -
 initialise all structs and variables

Signed-off-by: Roman <bapstroman@gmail.com>
---
 src/modules/commander/esc_calibration.cpp | 38 ++++++++++-------------
 src/modules/commander/esc_calibration.h   |  1 -
 2 files changed, 16 insertions(+), 23 deletions(-)

diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp
index 2e69ea97e2..039313b51b 100644
--- a/src/modules/commander/esc_calibration.cpp
+++ b/src/modules/commander/esc_calibration.cpp
@@ -61,25 +61,12 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/mavlink_log.h>
 
-int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) {
-	struct battery_status_s battery;
-	memset(&battery,0,sizeof(battery));
-	int batt_sub = orb_subscribe(ORB_ID(battery_status));
-	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
-
-	if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
-		mavlink_log_info(mavlink_log_pub, "Please disconnect battery and try again!");
-		return PX4_ERROR;
-	}
-	return PX4_OK;
-}
-
 int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
 {
 	int	return_code = PX4_OK;
 	
 #if defined(__PX4_POSIX_OCPOC)
-	hrt_abstime timeout_start;
+	hrt_abstime timeout_start = 0;
 	hrt_abstime timeout_wait = 60*1000*1000;
 	armed->in_esc_calibration_mode = true;
 	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc");
@@ -105,14 +92,14 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 #else
 	int	fd = -1;
 
-	struct	battery_status_s battery;
+	struct	battery_status_s battery = {};
 	int	batt_sub = -1;
 	bool	batt_updated = false;
-	bool	batt_connected = false;
+	bool	batt_connected = true;	// for safety resons assume battery is connected, will be cleared below if not the case
 
-	hrt_abstime battery_connect_wait_timeout = 30000000;
-	hrt_abstime pwm_high_timeout = 3000000;
-	hrt_abstime timeout_start;
+	hrt_abstime battery_connect_wait_timeout = 30*1000*1000;
+	hrt_abstime pwm_high_timeout = 3*1000*1000;
+	hrt_abstime timeout_start = 0;
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
 
@@ -123,8 +110,15 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 	}
 
 	// Make sure battery is disconnected
-	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
-	if (battery.voltage_filtered_v > 3.0f) {
+	if (orb_copy(ORB_ID(battery_status), batt_sub, &battery) == PX4_OK) {
+
+		// battery is not connected if voltage is lower than 3V and we have a recent battery measurement
+		if (battery.voltage_filtered_v < 3.0f && (hrt_absolute_time() - battery.timestamp < 500*1000)) {
+			batt_connected = false;
+		}
+	}
+
+	if (batt_connected) {
 		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
 		goto Error;
 	}
@@ -187,7 +181,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 				}
 			}
 		}
-		usleep(50000);
+		usleep(50*1000);
 	}
 
 Out:
diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h
index dd581ba2b4..c7c7b84bbf 100644
--- a/src/modules/commander/esc_calibration.h
+++ b/src/modules/commander/esc_calibration.h
@@ -44,7 +44,6 @@
 
 #include <uORB/topics/actuator_armed.h>
 
-int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub);
 int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed);
 
 #endif