WIP: BBSRAM check point vehicle_status_s

src/lib/systemlib/hardfault_log.h

@@ -38,21 +38,31 @@
 #include <px4_platform_common/px4_config.h>
 #include <systemlib/px4_macros.h>
 
+#include <uORB/topics/vehicle_status.h>
+
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
 #define HARDFAULT_REBOOT_FILENO 0
 #define HARDFAULT_REBOOT_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_REBOOT_FILENO)
+
+#define HARDFAULT_VEHICLE_STATUS_0_FILENO 1
+#define HARDFAULT_VEHICLE_STATUS_0_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_VEHICLE_STATUS_0_FILENO)
+
+#define HARDFAULT_VEHICLE_STATUS_1_FILENO 2
+#define HARDFAULT_VEHICLE_STATUS_1_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_VEHICLE_STATUS_1_FILENO)
+
 #define HARDFAULT_ULOG_FILENO 3
 #define HARDFAULT_ULOG_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_ULOG_FILENO)
+
 #define HARDFAULT_FILENO 4
 #define HARDFAULT_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_FILENO)
 
 #define HARDFAULT_MAX_ULOG_FILE_LEN 64 /* must be large enough to store the full path to the log file */
 
 #define BBSRAM_SIZE_FN0 (sizeof(int))
-#define BBSRAM_SIZE_FN1 384     /* greater then 2.5 times the size of vehicle_status_s */
-#define BBSRAM_SIZE_FN2 384     /* greater then 2.5 times the size of vehicle_status_s */
+#define BBSRAM_SIZE_FN1 (sizeof(struct vehicle_status_s))     /* greater then 2.5 times the size of vehicle_status_s */
+#define BBSRAM_SIZE_FN2 (sizeof(struct vehicle_status_s))     /* greater then 2.5 times the size of vehicle_status_s */
 #define BBSRAM_SIZE_FN3 HARDFAULT_MAX_ULOG_FILE_LEN
 #define BBSRAM_SIZE_FN4 -1
 

src/modules/commander/Commander.cpp

@@ -76,6 +76,10 @@
 #include <cstring>
 #include <matrix/math.hpp>
 
+#ifdef __PX4_NUTTX
+# include <lib/systemlib/hardfault_log.h>
+#endif // __PX4_NUTTX
+
 #include <uORB/topics/mavlink_log.h>
 
 typedef enum VEHICLE_MODE_FLAG {
@@ -681,6 +685,34 @@ Commander::Commander() :
 	ModuleParams(nullptr),
 	_failure_detector(this)
 {
+
+#if defined(px4_savepanic)
+	{
+		vehicle_status_s buf1{};
+		vehicle_status_s buf2{};
+
+		int fd1 = ::open(HARDFAULT_VEHICLE_STATUS_0_PATH, O_RDONLY);
+		int fd2 = ::open(HARDFAULT_VEHICLE_STATUS_1_PATH, O_RDONLY);
+
+		int read_ret1 = ::read(fd1, &buf1, sizeof(vehicle_status_s));
+		int read_ret2 = ::read(fd2, &buf2, sizeof(vehicle_status_s));
+
+		::close(fd1);
+		::close(fd1);
+
+		if ((read_ret1 == sizeof(vehicle_status_s)) && (read_ret2 == sizeof(vehicle_status_s)))
+		{
+			if (buf1.timestamp > buf2.timestamp) {
+				print_message(ORB_ID(vehicle_status), buf1);
+
+			} else if (buf2.timestamp > buf1.timestamp) {
+				print_message(ORB_ID(vehicle_status), buf2);
+			}
+		}
+	}
+#endif // px4_savepanic
+
+
 	_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
 
 	_land_detector.landed = true;
@@ -2856,6 +2888,9 @@ Commander::run()
 			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
 			_failure_detector_status_pub.publish(fd_status);
+
+
+			SaveCheckPoint();
 		}
 
 		/* play arming and battery warning tunes */
@@ -2962,6 +2997,35 @@ Commander::run()
 	buzzer_deinit();
 }
 
+void Commander::SaveCheckPoint()
+{
+#if defined(px4_savepanic)
+
+	_check_point_index = (_check_point_index == 0) ? 1 : 0;
+
+	int fd = -1;
+
+	if (_check_point_index == 0) {
+		fd = open(HARDFAULT_VEHICLE_STATUS_0_PATH, O_TRUNC | O_WRONLY | O_CREAT);
+
+	} else if (_check_point_index == 1) {
+		fd = open(HARDFAULT_VEHICLE_STATUS_1_PATH, O_TRUNC | O_WRONLY | O_CREAT);
+	}
+
+	if (fd < 0) {
+		return;
+	}
+
+	int write_ret = ::write(fd, (void *)&_status, sizeof(vehicle_status_s));
+	close(fd);
+
+	if (sizeof(vehicle_status_s) != write_ret) {
+		PX4_ERR("vehicle status check point failed ret=%d, errno=%d (%s)", write_ret, errno, strerror(errno));
+	}
+
+#endif // px4_savepanic
+}
+
 void
 Commander::get_circuit_breaker_params()
 {

src/modules/commander/Commander.hpp

@@ -129,6 +129,8 @@ private:
 
 	void battery_status_check();
 
+	void SaveCheckPoint();
+
 	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
 				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
 				   const bool was_valid);
@@ -397,6 +399,11 @@ private:
 
 	WorkerThread _worker_thread;
 
+#if defined(__PX4_NUTTX) && defined(px4_savepanic)
+	// check point
+	uint8_t _check_point_index{0};
+#endif // __PX4_NUTTX && px4_savepanic
+
 	// Subscriptions
 	uORB::Subscription					_action_request_sub {ORB_ID(action_request)};
 	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};