From dbbe3c0863bcf3167c1e973ccdedc2b03952b668 Mon Sep 17 00:00:00 2001
From: davidaroyer <dave@aerotenna.com>
Date: Mon, 27 Mar 2017 17:40:50 -0500
Subject: [PATCH] commander: add support for Aerotenna OcPoC-Zynq hardware

---
 src/modules/commander/PreflightCheck.cpp  |  3 +++
 src/modules/commander/esc_calibration.cpp | 26 +++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 324f79c7a4..586b67c09a 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -571,6 +571,9 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
 #elif defined(__PX4_POSIX_BEBOP)
 	PX4_WARN("Preflight checks always pass on Bebop.");
 	return true;
+#elif defined(__PX4_POSIX_OCPOC)
+	PX4_WARN("Preflight checks always pass on OcPoC.");
+	return true;
 #endif
 
 	bool failed = false;
diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp
index 20ff73e76c..8fbcfcb56c 100644
--- a/src/modules/commander/esc_calibration.cpp
+++ b/src/modules/commander/esc_calibration.cpp
@@ -77,7 +77,32 @@ 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)
 {
 	int	return_code = PX4_OK;
+	
+#if defined(__PX4_POSIX_OCPOC)
+	hrt_abstime timeout_start;
+	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");
+	timeout_start = hrt_absolute_time();
+	
+	while (true) {
+		if (hrt_absolute_time() - timeout_start > timeout_wait) {
+			break;
+		}else{
+			usleep(50000);
+		}
+	}
+
+	armed->in_esc_calibration_mode = false;
+	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "end esc");
+
+	if (return_code == OK) {
+		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
+	}
+		  
+	return return_code;
 
+#else
 	int	fd = -1;
 
 	struct	battery_status_s battery;
@@ -192,4 +217,5 @@ Out:
 Error:
 	return_code = PX4_ERROR;
 	goto Out;
+#endif
 }