AirLeash app (airdog module) changes

8 files changed, 296 insertions, 136 deletions

diff --git a/src/modules/airdog/airdog.cpp b/src/modules/airdog/airdog.cpp
index 47d6ab214..251a58112 100644
--- a/src/modules/airdog/airdog.cpp
+++ b/src/modules/airdog/airdog.cpp
@@ -154,29 +154,30 @@ void cAirdog::cycle()
 		armed = airdog_status.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
 		display_drone_state();
 
-		handle_takeoff();
+		// handle_takeoff();
 
-		if (airdog_status.battery_remaining < bat_warning_level && battery_warning_count < 3 && airdog_status.battery_remaining!= 0)
-		{
-			mavlink_log_info(mavlink_fd, "remaining %d", airdog_status.battery_remaining);
-			battery_warning_count++;
-			ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
-			pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_SLOW);
+		if(!hil) {
+			if (airdog_status.battery_remaining < bat_warning_level && battery_warning_count < 3 && airdog_status.battery_remaining!= 0)
+			{
+				mavlink_log_info(mavlink_fd, "remaining %d", airdog_status.battery_remaining);
+				battery_warning_count++;
+				ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+				pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_SLOW);
 
-		} else if (airdog_status.battery_remaining < bat_critical_level && airdog_status.battery_remaining!= 0)
-		{
-			if (!rtl_triggered_from_battery && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_RTL && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_LAND)
+			} else if (airdog_status.battery_remaining < bat_critical_level && airdog_status.battery_remaining!= 0)
+			{
+				if (!rtl_triggered_from_battery && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_RTL && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_LAND)
+				{
+					rtl_triggered_from_battery = true;
+					// send_set_state(NAV_STATE_RTL, MOVE_NONE);
+					pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_FAST);
+				}
+			} else if (airdog_status.battery_remaining > bat_warning_level)
 			{
-				rtl_triggered_from_battery = true;
-				// send_set_state(NAV_STATE_RTL, MOVE_NONE);
-				pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_FAST);
+				pi2c_ctrl->stop_blinking_led(I2C_LED_RED, true);
 			}
-		} else if (airdog_status.battery_remaining > bat_warning_level)
-		{
-			pi2c_ctrl->stop_blinking_led(I2C_LED_RED, true);
 		}
 
-
 		if (airdog_status.timestamp > 0)
 		{
 			drone_active = true;
@@ -230,6 +231,50 @@ void cAirdog::send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_
 	}
 }
 
+void cAirdog::send_set_auto_mode(uint8_t base_mode, enum PX4_CUSTOM_SUB_MODE_AUTO custom_sub_mode_auto)
+{
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* fill command */
+	cmd.command = VEHICLE_CMD_DO_SET_MODE;
+	cmd.confirmation = false;
+	cmd.param1 = base_mode;
+	cmd.param2 = custom_sub_mode_auto;
+	cmd.source_system = vehicle_status.system_id;
+	cmd.source_component = vehicle_status.component_id;
+	// TODO add parameters AD_VEH_SYSID, AD_VEH_COMP to set target id
+	cmd.target_system = 1;
+	cmd.target_component = 50;
+
+	if (cmd_pub < 0) {
+		cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	} else {
+		orb_publish(ORB_ID(vehicle_command), cmd_pub, &cmd);
+	}
+}
+
+void cAirdog::send_command(enum REMOTE_CMD command)
+{
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* fill command */
+	cmd.param1 = command;
+	cmd.command = VEHICLE_CMD_NAV_REMOTE_CMD;
+	cmd.confirmation = false;
+	cmd.source_system = vehicle_status.system_id;
+	cmd.source_component = vehicle_status.component_id;
+	cmd.target_system = 1;
+	cmd.target_component = 50;
+
+	if (cmd_pub < 0) {
+		cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	} else {
+		orb_publish(ORB_ID(vehicle_command), cmd_pub, &cmd);
+	}
+}
+
 // void cAirdog::send_set_state(enum NAV_STATE state, enum AUTO_MOVE_DIRECTION direction)
 // {
 // 	struct vehicle_command_s cmd;
@@ -323,74 +368,57 @@ bool cAirdog::button_clicked_i2c(uint8_t button, bool long_press)
 			break;
 		case 1:
 			// DOWN button
-			if (long_press) {
-				// send_set_state(NAV_STATE_RTL, MOVE_NONE);
-			} else {
-				// send_set_move(MOVE_DOWN);
-			}
+			send_command(REMOTE_CMD_DOWN);
 			break;
 		case 2:
 			// PLAY button
 
-			if (pitch_down)
-			{
-				pparam_handler->sendCustomParam("AIRD_PITCH_DOWN", PTYPE_INT, 0, true);
-				pitch_down = false;
-			}
 			if (!armed & drone_active)
 			{
-				if (long_press)
+				uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+				if (hil)
 				{
-					uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-					if (hil)
-					{
-						base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-					}
-					send_set_mode(base_mode, PX4_CUSTOM_MAIN_MODE_AUTO);
-					//while (airdog_status.main_mode != PX4_CUSTOM_MAIN_MODE_AUTO) {
-					//	System.Threading.Thread.Sleep(250); // pause for 1/4 second;
-					//};
-					takeoff_requested = true;
+					base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 				}
+				send_set_mode(base_mode, PX4_CUSTOM_MAIN_MODE_AUTO);
 			} else {
-				if (long_press)
-				{
-					if (pparam_handler->get(PARAM_NAV_LAND_HOME) > 0) {
-						// send_set_state(NAV_STATE_RTL, MOVE_NONE);
-					} else {
-						// send_set_state(NAV_STATE_LAND, MOVE_NONE);
-					}
-					//send_set_mode(MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_AUTO);
-				} else {
-					if (airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER)
-					{
-						// send_set_state(NAV_STATE_AFOLLOW, MOVE_NONE);
-					} else {
-						// send_set_state(NAV_STATE_LOITER, MOVE_NONE);
-					}
-				}
+				send_command(REMOTE_CMD_PLAY_PAUSE);
 			}
+			// } else {
+			// 	if (long_press)
+			// 	{
+			// 		if (pparam_handler->get(PARAM_NAV_LAND_HOME) > 0) {
+			// 			// send_set_state(NAV_STATE_RTL, MOVE_NONE);
+			// 		} else {
+			// 			// send_set_state(NAV_STATE_LAND, MOVE_NONE);
+			// 		}
+			// 		//send_set_mode(MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_AUTO);
+			// 	} else {
+			// 		if (airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER)
+			// 		{
+			// 			// send_set_state(NAV_STATE_AFOLLOW, MOVE_NONE);
+			// 		} else {
+			// 			// send_set_state(NAV_STATE_LOITER, MOVE_NONE);
+			// 		}
+			// 	}
+			// }
 			break;
 		case 3:
 			// UP button
-			if (long_press) {
-				set_land_mode();
-			} else {
-				// send_set_move(MOVE_UP);
-			}
+			send_command(REMOTE_CMD_UP);
 			break;
 		case 4:
 			// CENTER button
 			//
 			if (long_press) {
-				if (!log_running) {
-					send_record_path_cmd(true);
-					log_running = true;
-					pi2c_disp_ctrl->set_symbols(SYMBOL_L, SYMBOL_0, SYMBOL_EMPTY);
-				} else {
-					send_record_path_cmd(false);
-					log_running = false;
-				}
+				// if (!log_running) {
+				// 	send_record_path_cmd(true);
+				// 	log_running = true;
+				// 	pi2c_disp_ctrl->set_symbols(SYMBOL_L, SYMBOL_0, SYMBOL_EMPTY);
+				// } else {
+				// 	send_record_path_cmd(false);
+				// 	log_running = false;
+				// }
 			} else if (vehicle_status.system_id == trainer_remote_id) { //TODO get trainer remote id from drone
 				// send_set_state(NAV_STATE_COME_HERE, MOVE_TRAINER);
 			} else {
@@ -399,37 +427,59 @@ bool cAirdog::button_clicked_i2c(uint8_t button, bool long_press)
 			break;
 		case 5:
 			// CENTER DOWN
-
-			if (long_press)
-			{
-				pparam_handler->sendCustomParam("AIRD_PITCH_DOWN", PTYPE_INT, 1, false);
-				pitch_down = true;
-			} else {
-				// send_set_move(MOVE_CLOSER);
-			}
+			send_command(REMOTE_CMD_CLOSER);
 			break;
 		case 6:
 			// CENTER RIGHT
-			// send_set_move(MOVE_RIGHT);
+			send_command(REMOTE_CMD_RIGHT);
 			break;
 		case 7:
 			// CENTER UP
-			if (long_press) {
-				if (vehicle_status.system_id == trainer_remote_id) {
-					// send_set_state(NAV_STATE_COME_HERE, MOVE_TARGET);
-				}
-			} else {
-				// send_set_move(MOVE_FARTHER);
-			}
+			send_command(REMOTE_CMD_FURTHER);
 			break;
 		case 8:
 			// CENTER LEFT
-			if (long_press) {
-				display_discharged_mah();
-			} else {
-				// send_set_move(MOVE_LEFT);
+			send_command(REMOTE_CMD_LEFT);
+			break;
+		case 9:
+			// DOWN + CENTER
+			send_command(REMOTE_CMD_LAND_DISARM);
+			break;
+		case 10:
+			// UP + CENTER
+			send_command(REMOTE_CMD_TAKEOFF);
+			break;
+		case 11:
+			// CENTER DOWN + CENTER
+			// pparam_handler->sendCustomParam("AIRD_PITCH_DOWN", PTYPE_INT, 1, false);
+			// pitch_down = true;
+
+			send_command(REMOTE_CMD_LOOK_DOWN);
+			break;
+		case 12:
+		{
+			// CENTER RIGHT + CENTER
+			uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_AUTO_ENABLED;
+			if (hil)
+			{
+				base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 			}
+
+			send_set_auto_mode(base_mode, PX4_CUSTOM_SUB_MODE_AUTO_RTL);
+
+			break;
+		}
+		case 13:
+		{
+			// CENTER UP + CENTER
+			send_command(REMOTE_CMD_COME_TO_ME);
 			break;
+		}
+		case 14:
+			// CENTER LEFT + CENTER
+			break;
+
+
 	}
 
 	return BUTTON_HANDLED;
@@ -465,20 +515,20 @@ void cAirdog::display_drone_state() {
 
 void cAirdog::handle_takeoff()
 {
-	bool ready_for_takeoff = armed && airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY;
-	uint64_t now = hrt_absolute_time();
-	if (takeoff_request_time > 0 && (now - takeoff_request_time) / (1000 * 1000) > 1 && ready_for_takeoff) {
-		rtl_triggered_from_battery = false;
-		// send_set_state(NAV_STATE_TAKEOFF, MOVE_NONE);
-		takeoff_request_time = now;
-	} else if (takeoff_requested && ready_for_takeoff) {
-		rtl_triggered_from_battery = false;
-		// send_set_state(NAV_STATE_TAKEOFF, MOVE_NONE);
-		takeoff_requested = false;
-		takeoff_request_time = now;
-	} else if (takeoff_request_time > 0 && airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF) {
-		takeoff_request_time = 0;
-	}
+	// bool ready_for_takeoff = armed && airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY;
+	// uint64_t now = hrt_absolute_time();
+	// if (takeoff_request_time > 0 && (now - takeoff_request_time) / (1000 * 1000) > 1 && ready_for_takeoff) {
+	// 	rtl_triggered_from_battery = false;
+	// 	// send_set_state(NAV_STATE_TAKEOFF, MOVE_NONE);
+	// 	takeoff_request_time = now;
+	// } else if (takeoff_requested && ready_for_takeoff) {
+	// 	rtl_triggered_from_battery = false;
+	// 	// send_set_state(NAV_STATE_TAKEOFF, MOVE_NONE);
+	// 	takeoff_requested = false;
+	// 	takeoff_request_time = now;
+	// } else if (takeoff_request_time > 0 && airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF) {
+	// 	takeoff_request_time = 0;
+	// }
 }
 
 void cAirdog::display_discharged_mah() {
diff --git a/src/modules/airdog/airdog.h b/src/modules/airdog/airdog.h
index 0d7357970..28a40ac1e 100644
--- a/src/modules/airdog/airdog.h
+++ b/src/modules/airdog/airdog.h
@@ -69,6 +69,8 @@ private:
 	};
 
 	void send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_main_mode);
+	void send_set_auto_mode(uint8_t base_mode, enum PX4_CUSTOM_SUB_MODE_AUTO custom_sub_mode_auto);
+	void send_command(enum REMOTE_CMD command);
 	// void send_set_state(enum NAV_STATE state, enum AUTO_MOVE_DIRECTION direction);
 	// void send_set_move(enum AUTO_MOVE_DIRECTION direction);
 	void send_save_params();
diff --git a/src/modules/airdog/airdog_params.c b/src/modules/airdog/airdog_params.c
index 2873ca170..6b0539376 100644
--- a/src/modules/airdog/airdog_params.c
+++ b/src/modules/airdog/airdog_params.c
@@ -62,3 +62,9 @@ PARAM_DEFINE_INT32(AIRD_CHECK_MAG, 0);
 
 /*Enable automatic magnetic declination setting from coordinates*/
 PARAM_DEFINE_INT32(AIRD_AUTO_MAG, 1);
+
+/*Trajectory calculator low pass filter cutoff frequency (Hz) */
+PARAM_DEFINE_FLOAT(AIRD_TRAJ_CUT, 0.2f);
+
+/*Trajectory calculator stillness radius (meters) */
+PARAM_DEFINE_FLOAT(AIRD_TRAJ_RAD, 5);
diff --git a/src/modules/airdog/button_controller.cpp b/src/modules/airdog/button_controller.cpp
index 3a299bb33..a4671db8c 100644
--- a/src/modules/airdog/button_controller.cpp
+++ b/src/modules/airdog/button_controller.cpp
@@ -1,4 +1,5 @@
 #include "button_controller.h"
+#include <stdio.h>
 
 cButtonController::cButtonController(void)
 {
@@ -31,45 +32,104 @@ void cButtonController::check(enum button_set bs, struct button_s *buttons, int
 	cb_clicked = (bc_callback_clicked_t)callbacks[bs][BCBT_CLICKED];
 	cb_pressed = (bc_callback_pressed_t)callbacks[bs][BCBT_PRESSED];
 
-	for(int i = 0; i < count; i++) {
+	bool center_button_pressed = buttons[4].pressed;
+	hrt_abstime center_button_time_pressed = buttons[4].time_pressed;
+
+	for (int i = 0;i < count; i++){
+
 		now_pressed = !(mask[bs] & (1 << i));
-		if(!buttons[i].pressed) {
-			if(!now_pressed) {
-				continue;
-			}
-			if(ignore_mask[bs] & (1 << i)) {
-				//If now_pressed == true because of ignore_mask,
-				//unset ignore_mask (it's no more actual)
-				ignore_mask[bs] &= ~(1 << i);
-				continue;
-			} else {
-				buttons[i].time_pressed = now;
-				buttons[i].pressed = true;
-			}
+
+		// In last iteration wasn't pressed. Now it's pressed.
+		if (now_pressed && !buttons[i].pressed) {
+			buttons[i].time_pressed = now;
+			buttons[i].last_command_sent = 0;
+			buttons[i].pressed = true;
+		}
+
+		// Pressed buttons have to be handled first for menu to work.
+		if(cb_pressed && (*cb_pressed)(callback_args[bs][BCBT_CLICKED], i, now - buttons[i].time_pressed)) {
+			continue;
 		}
 
-		if(now_pressed) {
-			if(cb_pressed && (*cb_pressed)(callback_args[bs][BCBT_PRESSED], i, now - buttons[i].time_pressed)) {
-				//If pressed callback is available and it is handled
-				//do nothing
-				continue;
-			} else if(cb_clicked && now - buttons[i].time_pressed > LONG_PRESS_TIME) {
-				//If clicked callback is available and it is longpress,
-				//try to handle longpress now, not waiting for clicked event
-				if((*cb_clicked)(callback_args[bs][BCBT_CLICKED], i, true)) {
-					//If clicked callback is handled, setup ignore mask,
-					//so we wouldn't call clicked callback multiple times
-					buttons[i].pressed = false;
-					ignore_mask[bs] |= (1 << i);
-					mask[bs] &= ~(1 << i);
+		// In last iteration button was pressed. Now it's now.
+		if (!now_pressed && buttons[i].pressed) {
+
+
+			// Added this to not break long_press functionality for play button.
+			// Everywhere else long_press is not used any more.
+			/*
+			if (cb_clicked && i == 2)
+				(*cb_clicked)(callback_args[bs][BCBT_CLICKED], i, (now - buttons[i].time_pressed) > LONG_PRESS_TIME);
+			 */
+			buttons[i].pressed = false;
+		}
+
+
+		// Center button won't work alone, only in combination of others.
+		if (i!=4 && buttons[i].pressed)
+		{
+			// If center button is held down, all other buttons will have different functionality.
+			if (center_button_pressed && center_button_time_pressed < buttons[i].time_pressed) {
+
+				// In combination with center button, we don't need
+				// pressed button resend functionality
+
+				int new_button_number = 0;
+
+				switch (i) {
+					// DOWN + CENTER
+					case 1:
+						new_button_number = 9;
+						break;
+					// UP + CENTER
+					case 3:
+						new_button_number = 10;
+						break;
+					// CENTER DOWN + CENTER
+					case 5:
+						new_button_number = 11;
+						break;
+					// CENTER RIGHT + CENTER
+					case 6:
+						new_button_number = 12;
+						break;
+					// CENTER UP + CENTER
+					case 7:
+						new_button_number = 13;
+						break;
+					// CENTER LEFT + CENTER
+					case 8:
+						new_button_number = 14;
+						break;
+				}
+
+				if (new_button_number && buttons[i].last_command_sent == 0) {
+					(*cb_clicked)(callback_args[bs][BCBT_CLICKED], new_button_number, false);
+					buttons[i].last_command_sent = now;
+				}
+
+			// Handle buttons when center button is not pressed.
+			} else {
+
+				// Handle button press the first time
+				if (buttons[i].last_command_sent == 0) {
+					(*cb_clicked)(callback_args[bs][BCBT_CLICKED], i, false);
+					buttons[i].last_command_sent = now;
+				}
+
+				// Pressed button resend cycle
+				// Resend will work with:
+				// DOWN, UP, CENTER DONW, CENTER RIGHT, CENTER UP, CENTER LEFT
+				if ( i == 1 || i == 3 || i == 5 || i == 6 || i == 7 || i == 8) {
+
+					if ( now - buttons[i].last_command_sent > PRESSED_BUTTON_RESEND_CYCLE) {
+						(*cb_clicked)(callback_args[bs][BCBT_CLICKED], i, false);
+						buttons[i].last_command_sent = now;
+					}
 				}
 			}
-		} else {
-			if(cb_clicked) {
-				(*cb_clicked)(callback_args[bs][BCBT_PRESSED], i, (now - buttons[i].time_pressed) > LONG_PRESS_TIME);
-			}
-			buttons[i].pressed = false;
 		}
+
 	}
 }
 
diff --git a/src/modules/airdog/button_controller.h b/src/modules/airdog/button_controller.h
index 97b3e4a4e..938008363 100644
--- a/src/modules/airdog/button_controller.h
+++ b/src/modules/airdog/button_controller.h
@@ -70,6 +70,12 @@
  */
 #define LONG_PRESS_TIME 1500000
 
+/*
+ * Type: hrt_abstime
+ * Measured in microseconds
+ */
+#define PRESSED_BUTTON_RESEND_CYCLE 500000
+
 #define BUTTON_HANDLED true
 #define BUTTON_IGNORED false
 
@@ -112,6 +118,7 @@ private:
 	struct button_s {
 		bool pressed;
 		hrt_abstime time_pressed;
+		hrt_abstime last_command_sent;
 	};
 
 	void check(button_set bs, struct button_s *buttons, int count);
diff --git a/src/modules/airdog/menu_controller.cpp b/src/modules/airdog/menu_controller.cpp
index 41b19293f..ffa5873f7 100644
--- a/src/modules/airdog/menu_controller.cpp
+++ b/src/modules/airdog/menu_controller.cpp
@@ -61,6 +61,11 @@ void MENU_CONTROLLER::handleClickedButton(uint8_t button/*, bool long_press*/) {
 
 void MENU_CONTROLLER::open(void) {
 	if(!m_paramHandler->allParamsAreLoaded()) {
+
+        FILE * fh = fopen("/fs/microsd/log/m256", "a");
+        fprintf(fh, "Params not loaded!\n");
+        fclose(fh);
+
 		m_pi2c_disp_ctrl->set_symbols(SYMBOL_DOT, SYMBOL_DOT, SYMBOL_DOT);
 		return;
 	}
diff --git a/src/modules/airdog/paramhandler.cpp b/src/modules/airdog/paramhandler.cpp
index 8e7cf85f6..851a6fae8 100644
--- a/src/modules/airdog/paramhandler.cpp
+++ b/src/modules/airdog/paramhandler.cpp
@@ -58,20 +58,38 @@ void cParamHandler::setupSave(orb_advert_t *orb_cmd, int32_t system_id, int32_t
 }
 
 void cParamHandler::loadCycle(void) {
+
 	if(is_requested) {
 		int res;
 		bool updated;
-
+        
 		res = orb_check(passed_param_sub, &updated);
 		if(res == -1) {
 			//orb_check failed
+            //
+
+            FILE * fh = fopen("/fs/microsd/log/m256", "a");
+            fprintf(fh, "Orb check failed\n");
+            fclose(fh);
+
 		} else if(updated) {
+           
 			ad_param_t *pparam;
 			struct pass_drone_param_s param;
 			orb_copy(ORB_ID(pass_drone_parameter), passed_param_sub, &param);
 			pparam = &paramTable[loaded_count];
 
+
+            FILE * fh = fopen("/fs/microsd/log/m256", "a");
+            fprintf(fh, "Orb check ok\n");
+            fclose(fh);
+
 			if(!strncmp(param.param_id, pparam->name, sizeof(param.param_id))) {
+
+                FILE * fh = fopen("/fs/microsd/log/m256", "a");
+                fprintf(fh, "Compare ok \n");
+                fclose(fh);
+
 				pparam->type = (enum param_type)param.param_type;
 				switch(pparam->type) {
 					case PTYPE_INT:
@@ -90,6 +108,11 @@ void cParamHandler::loadCycle(void) {
 
 			is_requested = false;
 		} else if((hrt_absolute_time() - request_time) / 10000.0f > load_update_timeout) {
+
+            FILE * fh = fopen("/fs/microsd/log/m256", "a");
+            fprintf(fh, "Load update fail\n");
+            fclose(fh);
+
 			load_update_fail_count++;
 			if(load_update_fail_count >= max_load_update_fail_count) {
 				//TODO: Do something?
diff --git a/src/modules/airdog/paramhandler.h b/src/modules/airdog/paramhandler.h
index 460e22f50..d765c17df 100644
--- a/src/modules/airdog/paramhandler.h
+++ b/src/modules/airdog/paramhandler.h
@@ -55,7 +55,14 @@ public:
 	bool sendCustomParam(const char *name, enum param_type type, float new_value, bool upload);
 
 	//Menu controls
-	inline bool allParamsAreLoaded(void) { return loaded_count == PARAM_COUNT; }
+	inline bool allParamsAreLoaded(void) { 
+        
+        FILE * fh = fopen("/fs/microsd/log/m256", "a");
+        fprintf(fh, "Loaded count is %d, needed count is %d", loaded_count, PARAM_COUNT);
+        fclose(fh);
+
+        return loaded_count == PARAM_COUNT;
+    }
 	inline int getParamCount(void) { return PARAM_COUNT; }
 
 	void incParam(int param_id);