added airdog module

25 files changed, 2489 insertions, 0 deletions

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index dd028de7c..92f512287 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -76,6 +76,7 @@ MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
 MODULES		+= modules/uavcan
+MODULES 	+= modules/airdog
 
 #
 # Estimation modules (EKF/ SO3 / other filters)
diff --git a/src/modules/airdog/airdog.cpp b/src/modules/airdog/airdog.cpp
new file mode 100644
index 000000000..47d6ab214
--- /dev/null
+++ b/src/modules/airdog/airdog.cpp
@@ -0,0 +1,578 @@
+#include "airdog.h"
+
+extern "C" __EXPORT int airdog_main(int argc, char *argv[]);
+
+//For work queue, it requries void function
+static void airdog_cycle(void *arg);
+static void airdog_start(void *arg);
+
+//For button controller callbacks
+static bool airdog_button_pressed_i2c(void *arg, uint8_t button, hrt_abstime time);
+static bool airdog_button_clicked_i2c(void *arg, uint8_t button, bool long_press);
+
+cAirdog *g_pAirdog = nullptr;
+
+cAirdog::cAirdog() :
+	running(false),
+
+	base_mode(0),
+
+	airdog_status_sub(0),
+	vehicle_status_sub(0),
+
+	cmd_pub(-1),
+	cmd_log_start(-1),
+
+	buzzer(0),
+
+	pi2c_ctrl(nullptr),
+	pi2c_disp_ctrl(nullptr),
+	pmenu_ctrl(nullptr),
+	pparam_handler(nullptr),
+	pbutton_ctrl(nullptr),
+
+	mavlink_fd(0),
+	hil(false),
+	armed(false),
+	drone_active(false),
+	log_running(false),
+
+	last_drone_timestamp(0),
+	takeoff_request_time(0),
+
+	takeoff_requested(false),
+
+	bat_warning_level(0.0f),
+	bat_critical_level(0.0f),
+	trainer_remote_id(0),
+	pitch_down(false),
+
+	battery_warning_count(0)
+{
+	memset(&work, 0, sizeof(work));
+
+	memset(&airdog_status, 0, sizeof(airdog_status));
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+
+	memset(&param_bat_warn, 0, sizeof(param_bat_warn));
+	memset(&param_bat_failsafe, 0, sizeof(param_bat_failsafe));
+	memset(&param_trainer_id, 0, sizeof(param_trainer_id));
+}
+
+cAirdog::~cAirdog()
+{
+	running = false;
+	work_cancel(LPWORK, &work);
+	if(pmenu_ctrl != nullptr)
+		delete pmenu_ctrl;
+	if(pi2c_disp_ctrl != nullptr)
+		delete pi2c_disp_ctrl;
+	if(pi2c_ctrl != nullptr)
+		delete pi2c_ctrl;
+	if(pbutton_ctrl != nullptr)
+		delete pbutton_ctrl;
+	if(pparam_handler != nullptr)
+		delete pparam_handler;
+}
+
+void cAirdog::start()
+{
+	int ret;
+
+	base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
+
+	/* subscribe to vehicle status topic */
+	airdog_status_sub = orb_subscribe(ORB_ID(airdog_status));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
+	param_bat_warn = param_find("AIRD_BAT_WARN");
+	param_bat_failsafe = param_find("AIRD_BAT_FS");
+	param_trainer_id = param_find("AIRD_TRAINER_ID");
+
+	param_get(param_bat_warn, &bat_warning_level);
+	param_get(param_bat_failsafe, &bat_critical_level);
+	param_get(param_trainer_id, &trainer_remote_id);
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
+
+	//set to true on request (remote button press)
+	log_running = false;
+
+	pparam_handler = new cParamHandler();
+	if(pparam_handler == nullptr) errx(30, "airdog couldn't create param handler");
+	if(!pparam_handler->init()) {
+		errx(31, "airdog couldn't init param handler");
+	}
+	pparam_handler->setupSave(&cmd_pub, vehicle_status.system_id, vehicle_status.component_id);
+
+	pbutton_ctrl = new cButtonController();
+	if(pbutton_ctrl == nullptr) errx(38, "airdog couldn't create button controller");
+	pbutton_ctrl->setCallback(BS_I2C, BCBT_PRESSED, (void*)airdog_button_pressed_i2c, this);
+	pbutton_ctrl->setCallback(BS_I2C, BCBT_CLICKED, (void*)airdog_button_clicked_i2c, this);
+
+	pi2c_ctrl = new I2C_CONTROLLER(PX4_I2C_BUS_EXPANSION, LISTENER_ADDR);
+	if(pi2c_ctrl == nullptr) errx(32, "airdog couldn't create i2c controller");
+	if((ret = pi2c_ctrl->init(pbutton_ctrl)) != OK) {
+		errx(33, "airdog couldn't init i2c controller (%i)", ret);
+	}
+
+	pi2c_disp_ctrl = new I2C_DISPLAY_CONTROLLER(PX4_I2C_BUS_EXPANSION, DISPLAY_ADDR);
+	if(pi2c_disp_ctrl == nullptr) errx(34, "airdog couldn't create i2c display controller");
+	if((ret = pi2c_disp_ctrl->init()) != OK) {
+		errx(35, "airdog couldn't init i2c display controller (%i)", ret);
+	}
+
+	pi2c_disp_ctrl->clear_display();
+
+	pmenu_ctrl = new MENU_CONTROLLER(pi2c_disp_ctrl, pparam_handler);
+	if(pmenu_ctrl == nullptr) errx(36, "airdog couldn't create menu controller");
+
+	/* add worker to queue */
+	running = true;
+	ret = work_queue(LPWORK, &work, airdog_cycle, this, 0);
+	if (ret != OK) {
+		running = false;
+		errx(4, "airdog couldn't queue cycle work on start");
+	}
+}
+
+void cAirdog::cycle()
+{
+	bool updated;
+	orb_check(airdog_status_sub, &updated);
+
+	if (updated) {
+		pparam_handler->loadCycle();
+
+		orb_copy(ORB_ID(airdog_status), airdog_status_sub, &airdog_status);
+		hil = airdog_status.base_mode & MAV_MODE_FLAG_HIL_ENABLED;
+		armed = airdog_status.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
+		display_drone_state();
+
+		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);
+
+		} 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)
+		{
+			pi2c_ctrl->stop_blinking_led(I2C_LED_RED, true);
+		}
+
+
+		if (airdog_status.timestamp > 0)
+		{
+			drone_active = true;
+			pi2c_ctrl->set_red_led_on(true);
+			last_drone_timestamp = airdog_status.timestamp;
+		} else {
+			drone_active = false;
+			pi2c_ctrl->set_red_led_on(false);
+		}
+	}
+
+	uint64_t timeDiff = hrt_absolute_time() - last_drone_timestamp;
+	//mavlink_log_info(mavlink_fd,"time diff %llu", timeDiff);
+	if (timeDiff > 5000000)
+	{
+		drone_active = false;
+		pi2c_ctrl->set_red_led_on(false);
+	}
+
+	//warnx("connected %d, armed %d, hil %d, main mode %d, sub_mode %d",drone_active, _armed, _hil, airdog_status.main_mode, airdog_status.sub_mode);
+
+	pi2c_ctrl->cycle();
+	pbutton_ctrl->cycle();
+
+	/* repeat cycle at 10 Hz */
+	if (running) {
+		work_queue(LPWORK, &work, airdog_cycle, this, USEC2TICK(DEVICE_FREQUENCY));
+	}
+}
+
+void cAirdog::send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_main_mode)
+{
+	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_main_mode;
+	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_set_state(enum NAV_STATE state, enum AUTO_MOVE_DIRECTION direction)
+// {
+// 	struct vehicle_command_s cmd;
+// 	memset(&cmd, 0, sizeof(cmd));
+
+// 	/* fill command */
+// 	cmd.command = VEHICLE_CMD_NAV_SET_STATE;
+// 	cmd.param1 = state;
+// 	cmd.param2 = direction;
+// 	cmd.confirmation = false;
+// 	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_set_move(enum AUTO_MOVE_DIRECTION direction)
+// {
+// 	if (airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) {
+// 		send_set_state(NAV_STATE_LOITER, direction);
+// 	} else if (airdog_status.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_AFOLLOW) {
+// 		send_set_state(NAV_STATE_AFOLLOW, direction);
+// 	} else {
+// 		send_set_state(NAV_STATE_LOITER, direction);
+// 	}
+// }
+
+void cAirdog::send_record_path_cmd(bool start)
+{
+	struct airdog_path_log_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* fill command */
+	cmd.start = start;
+	cmd.stop = !start;
+
+	if (cmd_log_start < 0) {
+		cmd_log_start = orb_advertise(ORB_ID(airdog_path_log), &cmd);
+	} else {
+		orb_publish(ORB_ID(airdog_path_log), cmd_log_start, &cmd);
+	}
+}
+
+void cAirdog::set_land_mode()
+{
+	float value = pparam_handler->get(PARAM_NAV_LAND_HOME);
+	if(value > 0)
+		value = 0;
+	else
+		value = 1;
+
+	if(pparam_handler->send(PARAM_NAV_LAND_HOME, value, true)) {
+		if(value > 0)
+			pi2c_disp_ctrl->set_symbols(SYMBOL_EMPTY, SYMBOL_H, SYMBOL_E);
+		else
+			pi2c_disp_ctrl->set_symbols(SYMBOL_EMPTY, SYMBOL_H, SYMBOL_0);
+	} else {
+		pi2c_disp_ctrl->set_symbols(SYMBOL_E, SYMBOL_R, SYMBOL_R);
+	}
+}
+
+bool cAirdog::button_pressed_i2c(uint8_t button, hrt_abstime time)
+{
+	if(pmenu_ctrl->isActive()) {
+		pmenu_ctrl->handlePressedButton(button, time);
+		return BUTTON_HANDLED;
+	}
+	return BUTTON_IGNORED;
+}
+
+bool cAirdog::button_clicked_i2c(uint8_t button, bool long_press)
+{
+	if(pmenu_ctrl->isActive()) {
+		pmenu_ctrl->handleClickedButton(button/*, long_press*/);
+		return BUTTON_HANDLED;
+	}
+
+	switch(button) {
+		case 0:
+			// ON/OFF button
+			if (!long_press) {
+				pmenu_ctrl->open();
+			}
+			break;
+		case 1:
+			// DOWN button
+			if (long_press) {
+				// send_set_state(NAV_STATE_RTL, MOVE_NONE);
+			} else {
+				// send_set_move(MOVE_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)
+					{
+						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;
+				}
+			} 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);
+			}
+			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;
+				}
+			} 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 {
+				// send_set_state(NAV_STATE_COME_HERE, MOVE_TARGET);
+			}
+			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);
+			}
+			break;
+		case 6:
+			// CENTER RIGHT
+			// send_set_move(MOVE_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);
+			}
+			break;
+		case 8:
+			// CENTER LEFT
+			if (long_press) {
+				display_discharged_mah();
+			} else {
+				// send_set_move(MOVE_LEFT);
+			}
+			break;
+	}
+
+	return BUTTON_HANDLED;
+}
+
+void cAirdog::display_drone_state() {
+	if (pmenu_ctrl->isActive() || log_running) {
+		return;
+	}
+	if (!drone_active) {
+		pi2c_disp_ctrl->set_symbols(SYMBOL_EMPTY, SYMBOL_EMPTY, SYMBOL_EMPTY);
+	} else {
+		switch(airdog_status.sub_mode) {
+			case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
+				pi2c_disp_ctrl->set_symbols(SYMBOL_U, SYMBOL_P, SYMBOL_EMPTY);
+				break;
+			case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
+				pi2c_disp_ctrl->set_symbols(SYMBOL_L, SYMBOL_0, SYMBOL_1);
+				break;
+			// case PX4_CUSTOM_SUB_MODE_AUTO_AFOLLOW:
+			// 	pi2c_disp_ctrl->set_symbols(SYMBOL_F, SYMBOL_0, SYMBOL_L);
+			// 	break;
+			case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
+				pi2c_disp_ctrl->set_symbols(SYMBOL_L, SYMBOL_A, SYMBOL_EMPTY);
+				break;
+			default:
+				pi2c_disp_ctrl->set_symbols(SYMBOL_0, SYMBOL_F, SYMBOL_F);
+				break;
+		}
+	}
+
+}
+
+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;
+	}
+}
+
+void cAirdog::display_discharged_mah() {
+	if(airdog_status.discharged_mah > 0) {
+		pi2c_disp_ctrl->set_symbols_from_int(((int)airdog_status.discharged_mah) / 10);
+	} else {
+		pi2c_disp_ctrl->set_symbols(SYMBOL_A, SYMBOL_H, SYMBOL_E); //mAH Error
+	}
+	sleep(1);
+}
+
+static void airdog_cycle(void *arg) {
+	((cAirdog*)arg)->cycle();
+}
+
+static void airdog_start(void *arg) {
+	((cAirdog*)arg)->start();
+}
+
+static bool airdog_button_pressed_i2c(void *arg, uint8_t button, hrt_abstime time) {
+	return ((cAirdog*)arg)->button_pressed_i2c(button, time);
+}
+
+static bool airdog_button_clicked_i2c(void *arg, uint8_t button, bool long_press) {
+	return ((cAirdog*)arg)->button_clicked_i2c(button, long_press);
+}
+
+static void airdog_usage(const char *reason)
+{
+	if (reason)
+		warnx("%s\n", reason);
+	errx(1, "usage: airdog {start|stop|status} [-p <additional params>]\n\n");
+}
+
+int airdog_main(int argc, char *argv[])
+{
+	int res;
+	int buzzer = -1;
+	int rgbleds = -1;
+
+	if (argc < 1)
+		airdog_usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if(g_pAirdog != nullptr) {
+			errx(1, "airdog is already running");
+		}
+
+		g_pAirdog = new cAirdog();
+		if(g_pAirdog == nullptr) {
+			errx(2, "airdog failed to allocate class");
+		}
+
+		res = work_queue(LPWORK, &g_pAirdog->work, airdog_start, g_pAirdog, 0);
+
+		if(res != 0) {
+			errx(3, "airdog failed to queue work: %d", res);
+		} else {
+			warnx("airdog button listener starting");
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "alert")) {
+		warnx("Magnetometer not connected");
+
+        if (buzzer == -1) {
+            buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
+            rgbleds = open(RGBLED_DEVICE_PATH, 0);
+        }
+        ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
+        ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_BLINK_FAST);
+        ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_PURPLE);
+        exit(0);
+    }
+
+	if (g_pAirdog == nullptr)
+		errx(-1, "airdog is not running");
+
+	if (!strcmp(argv[1], "stop")) {
+		delete g_pAirdog;
+		g_pAirdog = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if(g_pAirdog != nullptr) {
+			warnx("\trunning\n");
+		} else {
+			warnx("\tnot started\n");
+		}
+		exit(0);
+	}
+
+	airdog_usage("unrecognized command");
+	exit(4);
+}
diff --git a/src/modules/airdog/airdog.h b/src/modules/airdog/airdog.h
new file mode 100644
index 000000000..0d7357970
--- /dev/null
+++ b/src/modules/airdog/airdog.h
@@ -0,0 +1,131 @@
+#ifndef AIRDOG_H
+#define AIRDOG_H
+
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <fcntl.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/airdog_status.h>
+#include <uORB/topics/airdog_path_log.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_rgbled.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_hrt.h>
+#include <commander/px4_custom_mode.h>
+#include <commander/commander_helper.h>
+#include <board_config.h>
+
+#include <mavlink/mavlink_log.h>
+
+#include "paramhandler.h"
+#include "button_controller.h"
+#include "i2c_controller.h"
+#include "i2c_display_controller.h"
+#include "menu_controller.h"
+#include "common.h"
+
+#define LISTENER_ADDR 0x20 /**< I2C adress of our button i2c controller */
+#define DISPLAY_ADDR 0x38 /**< I2C adress of our display i2c controller */
+
+class cAirdog
+{
+public:
+	cAirdog();
+	~cAirdog();
+
+	void start();
+	void cycle();
+
+	struct work_s work;
+
+	bool button_pressed_i2c(uint8_t button, hrt_abstime time);
+	bool button_clicked_i2c(uint8_t button, bool long_press);
+
+private:
+	enum MAV_MODE_FLAG {
+		MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1,		/* 0b00000001 Reserved for future use. | */
+		MAV_MODE_FLAG_TEST_ENABLED = 2,				/* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+		MAV_MODE_FLAG_AUTO_ENABLED = 4,				/* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+		MAV_MODE_FLAG_GUIDED_ENABLED = 8,			/* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+		MAV_MODE_FLAG_STABILIZE_ENABLED = 16,		/* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+		MAV_MODE_FLAG_HIL_ENABLED = 32,				/* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+		MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,	/* 0b01000000 remote control input is enabled. | */
+		MAV_MODE_FLAG_SAFETY_ARMED = 128,			/* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+	};
+
+	void send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_main_mode);
+	// 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();
+	void send_set_parameter_cmd(char *param_name, int int_val, bool should_save);
+	void send_get_parameter_value_cmd(char *param_name);
+	void send_record_path_cmd(bool start);
+	void set_land_mode();
+	void display_drone_state();
+	void load_param(int menu_param);
+	void set_param_if_updated();
+	void handle_takeoff();
+	void display_discharged_mah();
+
+	bool running;
+
+	uint8_t base_mode;
+	int airdog_status_sub;
+	int vehicle_status_sub;
+
+	orb_advert_t cmd_pub;
+	orb_advert_t cmd_log_start;
+
+	int buzzer;
+
+	I2C_CONTROLLER *pi2c_ctrl;
+	I2C_DISPLAY_CONTROLLER *pi2c_disp_ctrl;
+	MENU_CONTROLLER *pmenu_ctrl;
+	cParamHandler *pparam_handler;
+	cButtonController *pbutton_ctrl;
+
+	int mavlink_fd;
+	bool hil;
+	bool armed;
+	bool drone_active;
+	bool log_running;
+
+	struct airdog_status_s airdog_status;
+	struct vehicle_status_s vehicle_status;
+
+	uint64_t last_drone_timestamp;
+	uint64_t takeoff_request_time;
+
+	bool takeoff_requested;
+
+	param_t param_bat_warn;
+	param_t param_bat_failsafe;
+	param_t param_trainer_id;
+
+	float bat_warning_level;
+	float bat_critical_level;
+	bool rtl_triggered_from_battery;
+
+	int32_t trainer_remote_id;
+
+	bool pitch_down;
+
+	int battery_warning_count;
+};
+
+#endif // AIRDOG_H
diff --git a/src/modules/airdog/airdog_params.c b/src/modules/airdog/airdog_params.c
new file mode 100644
index 000000000..2873ca170
--- /dev/null
+++ b/src/modules/airdog/airdog_params.c
@@ -0,0 +1,64 @@
+/**
+ * @file airdog_params.c
+ * Airdog specific params.
+ *
+ * @author Kirils Sivokozs
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum altitude for manual follow
+ *
+ * Minimum altitude in manual follow. Recommended to be in range from 3 to 10.
+ *
+ * @min 0.0
+ * @max MAX_FLOAT
+ * @group Airdog
+ */
+PARAM_DEFINE_FLOAT(AIRD_MIN_MF_ALT, 3.0f);
+
+/**
+ * Airdog step to adjust position
+ *
+ * in LOITER mode
+ *
+ * @unit meters
+ * @group AirDog
+ */
+PARAM_DEFINE_FLOAT(AIRD_LOITER_STEP, 2.0f);
+
+/**
+ * Airdog step to adjust position
+ *
+ * in LOITER mode
+ *
+ * @unit meters
+ * @group AirDog
+ */
+PARAM_DEFINE_FLOAT(AIRD_BAT_WARN, 40.0f);
+
+/**
+ * Airdog step to adjust position
+ *
+ * in LOITER mode
+ *
+ * @unit meters
+ * @group AirDog
+ */
+PARAM_DEFINE_FLOAT(AIRD_BAT_FS, 10.0f);
+
+/*Custom binded device id.*/
+PARAM_DEFINE_INT32(AIRD_BINDED_ID, 4);
+
+/*Custom binded device id.*/
+PARAM_DEFINE_INT32(AIRD_TRAINER_ID, 10);
+
+/*Custom binded device id.*/
+PARAM_DEFINE_INT32(AIRD_PITCH_DOWN, 0);
+
+/*Enable if should check for external magnetometer*/
+PARAM_DEFINE_INT32(AIRD_CHECK_MAG, 0);
+
+/*Enable automatic magnetic declination setting from coordinates*/
+PARAM_DEFINE_INT32(AIRD_AUTO_MAG, 1);
diff --git a/src/modules/airdog/asciisymbolmap.h b/src/modules/airdog/asciisymbolmap.h
new file mode 100644
index 000000000..de2520729
--- /dev/null
+++ b/src/modules/airdog/asciisymbolmap.h
@@ -0,0 +1,135 @@
+#ifndef ASCIISYMBOLMAP_H
+#define ASCIISYMBOLMAP_H
+
+symbol_t symbolAsciiMap[128] = {
+	/*   0   */ SYMBOL_EMPTY,
+	/*   1 
 */ SYMBOL_EMPTY,
+	/*   2  */ SYMBOL_EMPTY,
+	/*   3  */ SYMBOL_EMPTY,
+	/*   4  */ SYMBOL_EMPTY,
+	/*   5   */ SYMBOL_EMPTY,
+	/*   6  */ SYMBOL_EMPTY,
+	/*   7   */ SYMBOL_EMPTY,
+	/*   8   */ SYMBOL_EMPTY,
+	/*   9 	 */ SYMBOL_EMPTY,
+	/*  10   */ SYMBOL_EMPTY,
+	/*  11   */ SYMBOL_EMPTY,
+	/*  12   */ SYMBOL_EMPTY,
+	/*  13   */ SYMBOL_EMPTY,
+	/*  14   */ SYMBOL_EMPTY,
+	/*  15   */ SYMBOL_EMPTY,
+	/*  16  */ SYMBOL_EMPTY,
+	/*  17  */ SYMBOL_EMPTY,
+	/*  18  */ SYMBOL_EMPTY,
+	/*  19  */ SYMBOL_EMPTY,
+	/*  20  */ SYMBOL_EMPTY,
+	/*  21  */ SYMBOL_EMPTY,
+	/*  22  */ SYMBOL_EMPTY,
+	/*  23  */ SYMBOL_EMPTY,
+	/*  24  */ SYMBOL_EMPTY,
+	/*  25  */ SYMBOL_EMPTY,
+	/*  26  */ SYMBOL_EMPTY,
+	/*  27  */ SYMBOL_EMPTY,
+	/*  28  */ SYMBOL_EMPTY,
+	/*  29  */ SYMBOL_EMPTY,
+	/*  30  */ SYMBOL_EMPTY,
+	/*  31  */ SYMBOL_EMPTY,
+	/*  32   */ SYMBOL_EMPTY,
+	/*  33 ! */ SYMBOL_EMPTY,
+	/*  34 " */ SYMBOL_EMPTY,
+	/*  35 # */ SYMBOL_EMPTY,
+	/*  36 $ */ SYMBOL_EMPTY,
+	/*  37 % */ SYMBOL_EMPTY,
+	/*  38 & */ SYMBOL_EMPTY,
+	/*  39 ' */ SYMBOL_EMPTY,
+	/*  40 ( */ SYMBOL_EMPTY,
+	/*  41 ) */ SYMBOL_EMPTY,
+	/*  42 * */ SYMBOL_EMPTY,
+	/*  43 + */ SYMBOL_EMPTY,
+	/*  44 , */ SYMBOL_EMPTY,
+	/*  45 - */ SYMBOL_MINUS,
+	/*  46 . */ SYMBOL_DOT,
+	/*  47 / */ SYMBOL_EMPTY,
+	/*  48 0 */ SYMBOL_0,
+	/*  49 1 */ SYMBOL_1,
+	/*  50 2 */ SYMBOL_2,
+	/*  51 3 */ SYMBOL_3,
+	/*  52 4 */ SYMBOL_4,
+	/*  53 5 */ SYMBOL_5,
+	/*  54 6 */ SYMBOL_6,
+	/*  55 7 */ SYMBOL_7,
+	/*  56 8 */ SYMBOL_8,
+	/*  57 9 */ SYMBOL_9,
+	/*  58 : */ SYMBOL_EMPTY,
+	/*  59 ; */ SYMBOL_EMPTY,
+	/*  60 < */ SYMBOL_EMPTY,
+	/*  61 = */ SYMBOL_EMPTY,
+	/*  62 > */ SYMBOL_EMPTY,
+	/*  63 ? */ SYMBOL_EMPTY,
+	/*  64 @ */ SYMBOL_EMPTY,
+	/*  65 A */ SYMBOL_A,
+	/*  66 B */ SYMBOL_B,
+	/*  67 C */ SYMBOL_C,
+	/*  68 D */ SYMBOL_D,
+	/*  69 E */ SYMBOL_E,
+	/*  70 F */ SYMBOL_F,
+	/*  71 G */ SYMBOL_EMPTY,
+	/*  72 H */ SYMBOL_H,
+	/*  73 I */ SYMBOL_EMPTY,
+	/*  74 J */ SYMBOL_EMPTY,
+	/*  75 K */ SYMBOL_EMPTY,
+	/*  76 L */ SYMBOL_L,
+	/*  77 M */ SYMBOL_EMPTY,
+	/*  78 N */ SYMBOL_EMPTY,
+	/*  79 O */ SYMBOL_EMPTY,
+	/*  80 P */ SYMBOL_P,
+	/*  81 Q */ SYMBOL_EMPTY,
+	/*  82 R */ SYMBOL_R,
+	/*  83 S */ SYMBOL_EMPTY,
+	/*  84 T */ SYMBOL_T,
+	/*  85 U */ SYMBOL_U,
+	/*  86 V */ SYMBOL_EMPTY,
+	/*  87 W */ SYMBOL_EMPTY,
+	/*  88 X */ SYMBOL_EMPTY,
+	/*  89 Y */ SYMBOL_Y,
+	/*  90 Z */ SYMBOL_EMPTY,
+	/*  91 [ */ SYMBOL_EMPTY,
+	/*  92 \ */ SYMBOL_EMPTY,
+	/*  93 ] */ SYMBOL_EMPTY,
+	/*  94 ^ */ SYMBOL_EMPTY,
+	/*  95 _ */ SYMBOL_EMPTY,
+	/*  96 ` */ SYMBOL_EMPTY,
+	/*  97 a */ SYMBOL_EMPTY,
+	/*  98 b */ SYMBOL_EMPTY,
+	/*  99 c */ SYMBOL_EMPTY,
+	/* 100 d */ SYMBOL_EMPTY,
+	/* 101 e */ SYMBOL_EMPTY,
+	/* 102 f */ SYMBOL_EMPTY,
+	/* 103 g */ SYMBOL_EMPTY,
+	/* 104 h */ SYMBOL_EMPTY,
+	/* 105 i */ SYMBOL_EMPTY,
+	/* 106 j */ SYMBOL_EMPTY,
+	/* 107 k */ SYMBOL_EMPTY,
+	/* 108 l */ SYMBOL_EMPTY,
+	/* 109 m */ SYMBOL_EMPTY,
+	/* 110 n */ SYMBOL_EMPTY,
+	/* 111 o */ SYMBOL_EMPTY,
+	/* 112 p */ SYMBOL_EMPTY,
+	/* 113 q */ SYMBOL_EMPTY,
+	/* 114 r */ SYMBOL_EMPTY,
+	/* 115 s */ SYMBOL_EMPTY,
+	/* 116 t */ SYMBOL_EMPTY,
+	/* 117 u */ SYMBOL_EMPTY,
+	/* 118 v */ SYMBOL_EMPTY,
+	/* 119 w */ SYMBOL_EMPTY,
+	/* 120 x */ SYMBOL_EMPTY,
+	/* 121 y */ SYMBOL_EMPTY,
+	/* 122 z */ SYMBOL_EMPTY,
+	/* 123 { */ SYMBOL_EMPTY,
+	/* 124 | */ SYMBOL_EMPTY,
+	/* 125 } */ SYMBOL_EMPTY,
+	/* 126 ~ */ SYMBOL_EMPTY,
+	/* 127  */ SYMBOL_EMPTY,
+};
+
+#endif // ASCIISYMBOLMAP_H
diff --git a/src/modules/airdog/button_controller.cpp b/src/modules/airdog/button_controller.cpp
new file mode 100644
index 000000000..3a299bb33
--- /dev/null
+++ b/src/modules/airdog/button_controller.cpp
@@ -0,0 +1,79 @@
+#include "button_controller.h"
+
+cButtonController::cButtonController(void)
+{
+	memset(bi2c, 0, sizeof(bi2c));
+
+	memset(callbacks, 0, sizeof(callbacks));
+	memset(callback_args, 0, sizeof(callback_args));
+
+	memset(mask, 0, sizeof(mask));
+	memset(ignore_mask, 0, sizeof(ignore_mask));
+}
+
+void cButtonController::setMask(button_set bs, uint32_t m) {
+	mask[bs] = m ^ ignore_mask[bs];
+}
+
+void cButtonController::cycle(void) {
+	//I2C button mask is set in i2c_controller
+	check(BS_I2C, bi2c, ARRAY_SIZE(bi2c));
+}
+
+void cButtonController::check(enum button_set bs, struct button_s *buttons, int count) {
+	hrt_abstime now;
+	bool now_pressed;
+	bc_callback_clicked_t cb_clicked;
+	bc_callback_pressed_t cb_pressed;
+
+	now = hrt_absolute_time();
+
+	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++) {
+		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;
+			}
+		}
+
+		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);
+				}
+			}
+		} else {
+			if(cb_clicked) {
+				(*cb_clicked)(callback_args[bs][BCBT_PRESSED], i, (now - buttons[i].time_pressed) > LONG_PRESS_TIME);
+			}
+			buttons[i].pressed = false;
+		}
+	}
+}
+
+void cButtonController::setCallback(enum button_set bs, enum button_callback_type bct, void *cb, void *arg) {
+	callbacks[bs][bct] = cb;
+	callback_args[bs][bct] = arg;
+}
diff --git a/src/modules/airdog/button_controller.h b/src/modules/airdog/button_controller.h
new file mode 100644
index 000000000..97b3e4a4e
--- /dev/null
+++ b/src/modules/airdog/button_controller.h
@@ -0,0 +1,128 @@
+/*
+ * This class handles different sets of buttons according to their
+ * previous and current status (pressed / not pressed), and duration of pressing a button.
+ *
+ * Duration of pressing a button is used to distinguish
+ * button click and longpress (i.e. long button press)
+ *
+ * Status of all buttons in a set is defined by the bitmap variable `mask`, which
+ * can be set using method `cButtonController::setMask(button_set bs, uint32_t bitmask)`
+ * So count of buttons in a set is limited to number of bits in uint32_t (i.e. 32)
+ * Bitmap variable `mask` is defined as follows:
+ *	- Bit N is set to **1** if N'th button is **not pressed**
+ *	- Bit N is set to **0** if N'th button is **pressed**
+ *
+ * Each set of buttons is checked in the method `cButtonController::cycle` by
+ * calling `cButtonController::check(button_set, array_of_buttons, count_of_buttons)`
+ * for each button set.
+ *
+ * Each button is handled by a callback that is defined for the button set.
+ * First two arguments of a callback are:
+ *	void*	- optional argument	(meant to be used to pass `this` variable
+ *								 if callback method is located in a class)
+ *	uint8_t	- number of a button
+ *
+ * Callback must return:
+ *	BUTTON_HANDLED (defined as true)	- if the button *was successfully handled*
+ *										- and *no* other callback must be called.
+ *	BUTTON_IGNORED (defined as false)	- if the button *was not handled*
+ *										- and another callback can be called.
+ *
+ * There are two types of callbacks:
+ *  - Clicked:	called when a button was pressed in the previous iteration of cycle
+ *				but is no longer pressed in the current iteration of cycle.
+ *				(i.e. button pressed and released)
+ *				Accepts additional argument:
+ *					- `bool longpress`: whether the button was pressed longer than _LONG_PRESS_TIME_
+ *
+ *  - Pressed:	called when a button is pressed first time and each iteration of cycle.
+ *				Accepts additional arguent:
+ *					- `hrt_abstime time`: duration pressing the button
+ *
+ * If callback pressed returns BUTTON_IGNORED and
+ * callback clicked is set and duration of pressing the button exceeds _LONG_PRESS_TIME_ then
+ * callback **clicked** is called with argument `longpress=true`, if it is handled then
+ * `ignore_mask` is set to ignore the pressed button in next cycle iterations untill it is released.
+ *
+ * There are 5 possible conditions of a button,
+ * only 4 of them are handled:
++-------------------+---------------------+---------------------+
+|	   Status       |     NOT pressed     |     WAS pressed     |
+|  Current\Previous |                     |                     |
++-------------------+---------------------+---------------------+
+|    NOT pressed    |               	  |      callback:      |
+|                   |  nothing to handle  | clicked(t >= LPTIME)|
++-------------------+---------------------+---------------------+
+|     IS pressed    |                     |      callback:      |
+|      < LPTIME     |	   callback:      |       pressed       |
++-------------------+       pressed       +---------------------+
+|     IS pressed    |     (time = 0)      |      callback:      |
+|     >= LPTIME     |                     |    clicked(true)    |
++-------------------+---------------------+---------------------+
+ */
+
+#ifndef BUTTON_CONTROLLER_H
+#define BUTTON_CONTROLLER_H
+
+/*
+ * Type: hrt_abstime
+ * Measured in microseconds
+ */
+#define LONG_PRESS_TIME 1500000
+
+#define BUTTON_HANDLED true
+#define BUTTON_IGNORED false
+
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
+
+#include "common.h"
+
+enum button_set {
+	BS_I2C = 0,
+	BS_COUNT
+};
+
+enum button_callback_type {
+	BCBT_PRESSED = 0,
+	BCBT_CLICKED,
+	BCBT_COUNT
+};
+
+typedef bool (*bc_callback_pressed_t)(void *arg, uint8_t button, hrt_abstime time);
+typedef bool (*bc_callback_clicked_t)(void *arg, uint8_t button, bool longpress);
+
+class cButtonController
+{
+public:
+	cButtonController(void);
+
+	//Checks all sets of buttons and calls representative callback
+	void cycle(void);
+
+	//Sets bitmap mask of a button set
+	void setMask(enum button_set bs, uint32_t m);
+
+
+	void setCallback(enum button_set bs, enum button_callback_type bct, void *cb, void *arg = nullptr);
+
+private:
+	struct button_s {
+		bool pressed;
+		hrt_abstime time_pressed;
+	};
+
+	void check(button_set bs, struct button_s *buttons, int count);
+
+	struct button_s bi2c[BUTTON_COUNT_I2C];
+
+	void *callbacks[BS_COUNT][BCBT_COUNT];
+	void *callback_args[BS_COUNT][BCBT_COUNT];
+
+	uint32_t mask[BS_COUNT];
+	uint32_t ignore_mask[BS_COUNT];
+};
+
+#endif // BUTTON_CONTROLLER_H
diff --git a/src/modules/airdog/common.h b/src/modules/airdog/common.h
new file mode 100644
index 000000000..8aab7056f
--- /dev/null
+++ b/src/modules/airdog/common.h
@@ -0,0 +1,55 @@
+/*
+ *
+ * Shared constants and enums
+ *
+ */
+
+#ifndef HAVE_ENUM_MAV_PARAM_TYPE
+#define HAVE_ENUM_MAV_PARAM_TYPE
+
+#include <unistd.h>
+
+#define DEVICE_FREQUENCY 100000
+
+enum MAV_PARAM_TYPE
+{
+	MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */
+	MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */
+	MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */
+	MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */
+	MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */
+	MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */
+	MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */
+	MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */
+	MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */
+	MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */
+	MAV_PARAM_TYPE_ENUM_END=11, /*  | */
+};
+
+typedef enum {
+    BLINKING_RATE_SLOW = 1000, // 1 Hz
+    BLINKING_RATE_MEDIUM = 500, // 2 Hz
+    BLINKING_RATE_FAST = 250, // 4 Hz
+} blinking_rate_t;
+
+typedef enum {
+    I2C_LED_GREEN = 0,
+    I2C_LED_RED,
+} i2c_led_t;
+
+struct led_s {
+    i2c_led_t led;
+    blinking_rate_t rate;
+    uint64_t last_blink;
+    bool should_blink;
+    bool was_on;
+};
+
+#define BUTTON_COUNT_I2C 9
+#define BUTTON_COUNT_GPIO 6
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a)/sizeof(a[0]))
+#endif
+
+#endif 
diff --git a/src/modules/airdog/i2c_controller.cpp b/src/modules/airdog/i2c_controller.cpp
new file mode 100644
index 000000000..1b7070354
--- /dev/null
+++ b/src/modules/airdog/i2c_controller.cpp
@@ -0,0 +1,223 @@
+#include "i2c_controller.h"
+
+#include <nuttx/config.h>
+#include <nuttx/clock.h> 
+
+#include <drivers/drv_hrt.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#define CONTROLLER_PATH "/dev/buttons"
+#define CONTROLLER_NAME "buttons"
+#define CONTROLLER_SETUP_COMMAND 0x00
+#define LED_SETUP_PORT_NUMBER 0x07
+#define LED_SETUP_CMD 0x3F
+#define LED_WRITE_PORT_NUMBER 0x03
+#define MSECS_IN_SEC 1000
+
+I2C_CONTROLLER::I2C_CONTROLLER(int bus, int addr) :
+	I2C(CONTROLLER_NAME, CONTROLLER_PATH, bus, addr, DEVICE_FREQUENCY),
+    _listening_interval(USEC2TICK(DEVICE_FREQUENCY))
+{
+    memset(&_green_led, 0, sizeof(_green_led));
+    memset(&_red_led, 0, sizeof(_red_led));
+}
+
+int
+I2C_CONTROLLER::init(cButtonController *pbutton_ctrl)
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	m_pbutton_ctrl = pbutton_ctrl;
+
+    ret = init_led();
+    if (ret != OK) {
+        return ret;
+    }
+
+    _led_update_in_progress = false;
+
+    _green_led = {I2C_LED_GREEN, BLINKING_RATE_SLOW, 0, false, false};
+    _red_led = {I2C_LED_RED, BLINKING_RATE_SLOW, 0, false, false};
+
+	return OK;
+}
+
+int
+I2C_CONTROLLER::probe()
+{
+    uint8_t response[1] = {0};
+    uint8_t requests[1] = {CONTROLLER_SETUP_COMMAND};
+
+    int ret = transfer(&requests[0], sizeof(requests), nullptr, 0);
+    ret = transfer(nullptr, 0, response, sizeof(response));
+	return ret;
+}
+
+int
+I2C_CONTROLLER::init_led()
+{
+    uint8_t requests[2] = {LED_SETUP_PORT_NUMBER, LED_SETUP_CMD};
+    int ret = transfer(requests, sizeof(requests), nullptr, 0);
+    return ret;
+}
+
+void
+I2C_CONTROLLER::cycle()
+{
+    if (!_led_update_in_progress) {
+		uint32_t button_mask = 0;
+
+		button_mask |= get_buttons_state_from_r1() << 0;
+		button_mask |= get_buttons_state_from_r2() << 8;
+
+		m_pbutton_ctrl->setMask(BS_I2C, button_mask);
+    }
+
+    uint64_t now = hrt_absolute_time();
+    if (_green_led.should_blink) {
+        if ((now - _green_led.last_blink) / MSECS_IN_SEC < _green_led.rate) {
+            set_green_led_on(false);
+        } else {
+            set_green_led_on(true);
+            _green_led.last_blink = now;
+        }
+    }
+
+    if (_red_led.should_blink) {
+        if ((now - _red_led.last_blink) / MSECS_IN_SEC < _red_led.rate) {
+            set_red_led_on(false);
+        } else {
+            set_red_led_on(true);
+            _red_led.last_blink = now;
+        }
+    }
+}
+
+int
+I2C_CONTROLLER::get_buttons_state_from_r1()
+{
+    uint8_t response[1] = {0};
+    // I do not know why it is 0x00, but it works
+    uint8_t requests[1] = {0x00};
+
+    int ret = transfer(&requests[0], sizeof(requests), nullptr, 0);
+    ret = transfer(nullptr, 0, response, sizeof(response)); 
+
+    if (ret == OK) {
+        return response[0];
+    }
+	return -1;
+}
+
+int
+I2C_CONTROLLER::get_buttons_state_from_r2()
+{
+    uint8_t response[1] = {0};
+    // This is according to specs. Fishy.
+    uint8_t requests[2] = {0x00, 0x41};
+
+    int ret = transfer(requests, sizeof(requests), nullptr, 0);
+    ret = transfer(nullptr, 0, response, sizeof(response)); 
+
+    if (ret == OK) {
+        return response[0];
+    }
+	return -1;
+}
+
+void
+I2C_CONTROLLER::set_green_led_on(bool set_on)
+{
+    uint8_t green = set_on ? 0x40 : 0x00;
+    uint8_t red = _is_red_led_on ? 0x80 : 0x00;
+    uint8_t state = 0xFF ^ green ^ red;
+
+    uint8_t requests[2] = {LED_WRITE_PORT_NUMBER, state};
+    int ret = transfer(requests, sizeof(requests), nullptr, 0);
+    if (ret == OK) {
+       _is_green_led_on = set_on;
+    }
+}
+
+void
+I2C_CONTROLLER::set_red_led_on(bool set_on)
+{
+    uint8_t green = _is_green_led_on ? 0x40 : 0x00;
+    uint8_t red = set_on ? 0x80 : 0x00;
+    uint8_t state = 0xFF ^ green ^ red;
+
+    uint8_t requests[2] = {LED_WRITE_PORT_NUMBER, state};
+    int ret = transfer(requests, sizeof(requests), nullptr, 0);
+    if (ret == OK) {
+       _is_red_led_on = set_on;
+    }
+}
+
+/*
+ * BLinking API
+ */
+
+void
+I2C_CONTROLLER::start_blinking_led(i2c_led_t led, blinking_rate_t rate)
+{
+    uint64_t now = hrt_absolute_time();
+    switch (led) {
+        case I2C_LED_GREEN:
+            if (_green_led.should_blink) {
+                return;
+            }
+            _green_led.rate = rate;
+            _green_led.should_blink = true;
+            _green_led.was_on = _is_green_led_on;
+            _green_led.last_blink = _red_led.last_blink == 0 ? now : _red_led.last_blink;
+            break;
+        case I2C_LED_RED:
+            if (_red_led.should_blink) {
+                return;
+            }
+            _red_led.rate = rate;
+            _red_led.should_blink = true;
+            _red_led.was_on = _is_red_led_on;
+            _red_led.last_blink = _green_led.last_blink == 0 ? now : _green_led.last_blink;
+            break;
+        default:
+            break;
+    }
+}
+
+void
+I2C_CONTROLLER::stop_blinking_led(i2c_led_t led, bool should_restore_state)
+{
+    switch (led) {
+        case I2C_LED_GREEN:
+            _green_led.should_blink = false;
+            _green_led.last_blink = 0;
+            set_red_led_on(_green_led.was_on);
+            break;
+        case I2C_LED_RED:
+            _red_led.should_blink = false;
+            _red_led.last_blink = 0;
+            set_green_led_on(_red_led.was_on);
+            break;
+        default:
+            break;
+    }
+}
diff --git a/src/modules/airdog/i2c_controller.h b/src/modules/airdog/i2c_controller.h
new file mode 100644
index 000000000..585851503
--- /dev/null
+++ b/src/modules/airdog/i2c_controller.h
@@ -0,0 +1,40 @@
+#ifndef _I2C_CONTROLLER_H
+#define _I2C_CONTROLLER_H
+
+#include <drivers/device/i2c.h>
+
+#include "button_controller.h"
+#include "common.h"
+
+class __EXPORT I2C_CONTROLLER : public device::I2C
+{
+public:
+	I2C_CONTROLLER(int bus, int addr);
+
+	int				init(cButtonController *pbutton_ctrl);
+	int				probe();
+
+	void			cycle();
+
+    void            start_blinking_led(i2c_led_t led, blinking_rate_t rate);
+    void            stop_blinking_led(i2c_led_t led, bool should_restore_state);
+    void            set_green_led_on(bool set_on);
+    void            set_red_led_on(bool set_on);
+
+private:
+	cButtonController *m_pbutton_ctrl;
+
+    led_s           _green_led;
+    led_s           _red_led;
+    bool            _led_update_in_progress;
+    int			    _listening_interval;
+
+    bool            _is_red_led_on;
+    bool            _is_green_led_on;
+
+    int             get_buttons_state_from_r1();
+    int             get_buttons_state_from_r2();
+    int             init_led();
+};
+
+#endif
diff --git a/src/modules/airdog/i2c_display_controller.cpp b/src/modules/airdog/i2c_display_controller.cpp
new file mode 100644
index 000000000..7592ab15d
--- /dev/null
+++ b/src/modules/airdog/i2c_display_controller.cpp
@@ -0,0 +1,123 @@
+#include "i2c_display_controller.h"
+
+#include <nuttx/config.h>
+#include <nuttx/clock.h> 
+
+#include <drivers/drv_hrt.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#define I2C_BUTTON_COUNT 9
+#define DISPLAY_PATH "/dev/display"
+#define DISPLAY_NAME "display"
+#define SET_MODE_CMD 0x4D
+#define WRITE_DATA_CMD 0x00
+
+I2C_DISPLAY_CONTROLLER::I2C_DISPLAY_CONTROLLER(int bus, int addr) :
+    I2C(DISPLAY_NAME, DISPLAY_PATH, bus, addr, DEVICE_FREQUENCY)
+{
+}
+
+I2C_DISPLAY_CONTROLLER::~I2C_DISPLAY_CONTROLLER()
+{
+}
+
+int
+I2C_DISPLAY_CONTROLLER::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	return OK;
+}
+
+int
+I2C_DISPLAY_CONTROLLER::probe()
+{
+    uint8_t requests[1] = {SET_MODE_CMD};
+
+    int ret = transfer(&requests[0], sizeof(requests), nullptr, 0);
+	return ret;
+}
+
+int
+I2C_DISPLAY_CONTROLLER::set_symbols(uint8_t first, uint8_t second, uint8_t third)
+{
+    uint8_t requests[4] = {WRITE_DATA_CMD, first, second, third};
+    return transfer(requests, sizeof(requests), nullptr, 0);
+}
+
+int
+I2C_DISPLAY_CONTROLLER::clear_display()
+{
+    return set_symbols(SYMBOL_EMPTY, SYMBOL_EMPTY, SYMBOL_EMPTY);
+}
+
+void
+I2C_DISPLAY_CONTROLLER::set_symbols_from_int(int number)
+{
+	symbol_t symbols[] = {
+		map_char_to_symbol('0' + ((number / 100) % 10)),
+		map_char_to_symbol('0' + ((number / 10) % 10)),
+		map_char_to_symbol('0' + ((number) % 10))
+	};
+	for(int	i = 0; i < 2; i++) {
+		if(symbols[i] == SYMBOL_0)
+			symbols[i] = SYMBOL_EMPTY;
+		else
+			break;
+	}
+	set_symbols(symbols[0], symbols[1], symbols[2]);
+}
+
+void
+I2C_DISPLAY_CONTROLLER::set_symbols_from_float(float number)
+{
+	char str[16];
+	int i, j, len;
+
+	len = sprintf(str, "%.2f", (double)number);
+
+	uint8_t symbols[3] = {SYMBOL_EMPTY, SYMBOL_EMPTY,SYMBOL_EMPTY};
+
+	for(i = 0, j = 0; i < len && j < 3; i++) {
+		symbols[j] = map_char_to_symbol(str[i]);
+		if(symbols[j] == SYMBOL_DOT) { //cannot be true when i == 0
+			symbols[j - 1] |= SYMBOL_DOT;
+		} else {
+			j++;
+		}
+	}
+	set_symbols(symbols[0], symbols[1], symbols[2]);
+}
+
+void
+I2C_DISPLAY_CONTROLLER::set_symbols_from_str(const char str[3])
+{
+	set_symbols(
+			map_char_to_symbol(str[0]),
+			map_char_to_symbol(str[1]),
+			map_char_to_symbol(str[2]));
+}
+
+symbol_t
+I2C_DISPLAY_CONTROLLER::map_char_to_symbol(char c)
+{
+	return symbolAsciiMap[c];
+}
diff --git a/src/modules/airdog/i2c_display_controller.h b/src/modules/airdog/i2c_display_controller.h
new file mode 100644
index 000000000..6c2063e2f
--- /dev/null
+++ b/src/modules/airdog/i2c_display_controller.h
@@ -0,0 +1,56 @@
+#ifndef _I2C_DISPLAY_CONTROLLER_H
+#define _I2C_DISPLAY_CONTROLLER_H
+
+#include <drivers/device/i2c.h>
+#include "common.h"
+
+typedef enum {
+	SYMBOL_0 = 0xFC,
+	SYMBOL_1 = 0x60,
+	SYMBOL_2 = 0xDA,
+	SYMBOL_3 = 0xF2,
+	SYMBOL_4 = 0x66,
+	SYMBOL_5 = 0xB6,
+	SYMBOL_6 = 0xBE,
+	SYMBOL_7 = 0xE0,
+	SYMBOL_8 = 0xFE,
+	SYMBOL_9 = 0xF6,
+	SYMBOL_A = 0xEE,
+	SYMBOL_U = 0x7C,
+	SYMBOL_Y = 0x76,
+	SYMBOL_F = 0x8E,
+	SYMBOL_P = 0xCE,
+	SYMBOL_L = 0x1C,
+	SYMBOL_B = 0x3E,
+	SYMBOL_H = 0x6E,
+	SYMBOL_E = 0x9E,
+	SYMBOL_D = 0x7A,
+	SYMBOL_C = 0x9C,
+	SYMBOL_T = 0x8C,
+	SYMBOL_R = 0x0A,
+	SYMBOL_EMPTY = 0x00,
+	SYMBOL_DOT = 0x01,
+	SYMBOL_MINUS = 0x02
+} symbol_t;
+
+
+class __EXPORT I2C_DISPLAY_CONTROLLER : public device::I2C
+{
+public:
+	I2C_DISPLAY_CONTROLLER(int bus, int addr);
+	virtual ~I2C_DISPLAY_CONTROLLER();
+
+	virtual int		init();
+	virtual int		probe();
+    int             set_symbols(uint8_t first, uint8_t second, uint8_t third);
+    int             clear_display();
+    void            set_symbols_from_int(int number);
+    void            set_symbols_from_float(float number);
+	void            set_symbols_from_str(const char str[3]);
+	symbol_t        map_char_to_symbol(char c);
+
+private:
+	#include "asciisymbolmap.h"
+};
+
+#endif 
diff --git a/src/modules/airdog/menu_controller.cpp b/src/modules/airdog/menu_controller.cpp
new file mode 100644
index 000000000..41b19293f
--- /dev/null
+++ b/src/modules/airdog/menu_controller.cpp
@@ -0,0 +1,135 @@
+#include "menu_controller.h"
+#include "common.h"
+
+#include <nuttx/config.h>
+#include <nuttx/clock.h> 
+
+#include <drivers/drv_hrt.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+MENU_CONTROLLER::MENU_CONTROLLER(I2C_DISPLAY_CONTROLLER *pi2c_disp_ctrl, cParamHandler *paramHandler) :
+	m_pi2c_disp_ctrl(pi2c_disp_ctrl),
+	m_paramHandler(paramHandler),
+	active(false),
+	currentLevel(MENU_LEVEL_PARAMS),
+	selectedParamId(0)
+{
+}
+
+void MENU_CONTROLLER::handlePressedButton(uint8_t button, hrt_abstime time) {
+	if(!m_paramHandler->allParamsAreLoaded())
+		return;
+
+	if(button >= BUTTON_COUNT_I2C)
+		return;
+
+	button_map_t cb = this->buttonMap[currentLevel][button];
+	switch(cb.type) {
+		case BCBT_PRESSED:
+			(this->*cb.func.pressed)(time);
+			break;
+	}
+}
+
+void MENU_CONTROLLER::handleClickedButton(uint8_t button/*, bool long_press*/) {
+	if(!m_paramHandler->allParamsAreLoaded())
+		return;
+
+	if(button >= BUTTON_COUNT_I2C)
+		return;
+
+	button_map_t cb = this->buttonMap[currentLevel][button];
+	switch(cb.type) {
+		case BCBT_CLICKED:
+			(this->*cb.func.clicked)(/*long_press*/);
+			break;
+	}
+}
+
+void MENU_CONTROLLER::open(void) {
+	if(!m_paramHandler->allParamsAreLoaded()) {
+		m_pi2c_disp_ctrl->set_symbols(SYMBOL_DOT, SYMBOL_DOT, SYMBOL_DOT);
+		return;
+	}
+	active = true;
+	displaySelectedParam();
+}
+
+void MENU_CONTROLLER::close(void) {
+	if(currentLevel == MENU_LEVEL_SET)
+		cancelEditing();
+	m_paramHandler->uploadAllParams();
+	active = false;
+}
+
+void MENU_CONTROLLER::displaySelectedParam(void) {
+	m_pi2c_disp_ctrl->set_symbols_from_str(m_paramHandler->getDisplaySymbols(selectedParamId));
+}
+
+void MENU_CONTROLLER::displaySelectedParamValue(void) {
+	switch(m_paramHandler->getEditingType(selectedParamId)) {
+		case PTYPE_INT:
+			m_pi2c_disp_ctrl->set_symbols_from_int(m_paramHandler->getEditingValue(selectedParamId));
+			break;
+		case PTYPE_FLOAT:
+			m_pi2c_disp_ctrl->set_symbols_from_float(m_paramHandler->getEditingValue(selectedParamId));
+			break;
+	}
+}
+
+void MENU_CONTROLLER::selectPrevParam(void) {
+	selectedParamId--;
+	if(selectedParamId < 0)
+		selectedParamId = m_paramHandler->getParamCount() - 1;
+	displaySelectedParam();
+}
+
+void MENU_CONTROLLER::selectNextParam(void) {
+	selectedParamId++;
+	if(selectedParamId >= m_paramHandler->getParamCount())
+		selectedParamId = 0;
+	displaySelectedParam();
+}
+
+void MENU_CONTROLLER::editCurrentParam(void) {
+	currentLevel = MENU_LEVEL_SET;
+	displaySelectedParamValue();
+}
+
+void MENU_CONTROLLER::decCurrentParam(hrt_abstime time) {
+	if(time == 0 || time > LONG_PRESS_TIME)
+		m_paramHandler->decParam(selectedParamId);
+	displaySelectedParamValue();
+}
+
+void MENU_CONTROLLER::incCurrentParam(hrt_abstime time) {
+	if(time == 0 || time > LONG_PRESS_TIME)
+		m_paramHandler->incParam(selectedParamId);
+	displaySelectedParamValue();
+}
+
+void MENU_CONTROLLER::cancelEditing(void) {
+	currentLevel = MENU_LEVEL_PARAMS;
+	m_paramHandler->resetParam(selectedParamId);
+	displaySelectedParam();
+}
+
+void MENU_CONTROLLER::saveCurrentParam(void) {
+	if(m_paramHandler->setParam(selectedParamId)) {
+		currentLevel = MENU_LEVEL_PARAMS;
+		displaySelectedParam();
+	}
+}
diff --git a/src/modules/airdog/menu_controller.h b/src/modules/airdog/menu_controller.h
new file mode 100644
index 000000000..2edf780b7
--- /dev/null
+++ b/src/modules/airdog/menu_controller.h
@@ -0,0 +1,91 @@
+#ifndef _MENU_CONTROLLER_H
+#define _MENU_CONTROLLER_H
+
+#include "i2c_display_controller.h"
+#include "button_controller.h"
+#include "paramhandler.h"
+#include <systemlib/err.h>
+
+#define BUTTON_ACTION(type, cb) ({ type, { &MENU_CONTROLLER::cb } })
+
+typedef enum {
+	MENU_LEVEL_PARAMS = 0,
+	MENU_LEVEL_SET,
+	MAX_MENU_LEVEL,
+} menu_level_t;
+
+class MENU_CONTROLLER
+{
+public:
+	MENU_CONTROLLER(I2C_DISPLAY_CONTROLLER *pi2c_disp_ctrl, cParamHandler *paramHandler);
+
+	void handlePressedButton(uint8_t button, hrt_abstime time);
+	void handleClickedButton(uint8_t button/*, bool long_press*/);
+
+	void open(void);
+	void close(void);
+
+	inline bool isActive(void) { return active; }
+
+private:
+	typedef void (MENU_CONTROLLER::*button_pressed_t)(hrt_abstime time);
+	typedef void (MENU_CONTROLLER::*button_clicked_t)(/*bool longpress*/);
+	typedef union {
+		button_pressed_t pressed;
+		button_clicked_t clicked;
+	} uni_button_callback_t;
+	typedef struct {
+		enum button_callback_type type;
+		uni_button_callback_t func;
+	} button_map_t;
+
+	I2C_DISPLAY_CONTROLLER *m_pi2c_disp_ctrl;
+	cParamHandler *m_paramHandler;
+
+	bool active;
+
+	menu_level_t currentLevel;
+
+	int selectedParamId;
+
+	void displaySelectedParam(void);
+	void displaySelectedParamValue(void);
+
+	void selectPrevParam(void);
+	void selectNextParam(void);
+	void editCurrentParam(void);
+
+	void decCurrentParam(hrt_abstime time);
+	void incCurrentParam(hrt_abstime time);
+	void cancelEditing(void);
+	void saveCurrentParam(void);
+
+	inline void nop(void) { }
+
+	button_map_t buttonMap[MAX_MENU_LEVEL][BUTTON_COUNT_I2C] = {
+		{ //Menu level PARAMS
+		  /* 0 ON/OFF */			{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::close } },
+		  /* 1 DOWN */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 2 PLAY */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 3 UP */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 4 CENTER */			{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::editCurrentParam } },
+		  /* 5 CENTER DOWN */		{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::selectPrevParam } },
+		  /* 6 CENTER RIGHT */		{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 7 CENTER UP */			{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::selectNextParam } },
+		  /* 8 CENTER LEFT */		{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		},
+		{ //Menu level SET
+		  /* 0 ON/OFF */			{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::close } },
+		  /* 1 DOWN */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 2 PLAY */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 3 UP */				{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 4 CENTER */			{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::saveCurrentParam } },
+		  /* 5 CENTER DOWN */		{ BCBT_PRESSED, { .pressed = &MENU_CONTROLLER::decCurrentParam } },
+		  /* 6 CENTER RIGHT */		{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::nop } },
+		  /* 7 CENTER UP */			{ BCBT_PRESSED, { .pressed = &MENU_CONTROLLER::incCurrentParam } },
+		  /* 8 CENTER LEFT */		{ BCBT_CLICKED, { .clicked = &MENU_CONTROLLER::cancelEditing } },
+		}
+	};
+};
+
+#endif //_MENU_CONTROLLER_H
diff --git a/src/modules/airdog/module.mk b/src/modules/airdog/module.mk
new file mode 100644
index 000000000..bc34e117f
--- /dev/null
+++ b/src/modules/airdog/module.mk
@@ -0,0 +1,10 @@
+MODULE_COMMAND = airdog
+SRCS = airdog.cpp \
+	   i2c_controller.cpp \
+	   i2c_display_controller.cpp \
+	   menu_controller.cpp \
+	   paramhandler.cpp \
+	   button_controller.cpp \
+	   airdog_params.c
+
+CFLAGS += -Wno-unknown-pragmas -Wno-packed
diff --git a/src/modules/airdog/paramhandler.cpp b/src/modules/airdog/paramhandler.cpp
new file mode 100644
index 000000000..8e7cf85f6
--- /dev/null
+++ b/src/modules/airdog/paramhandler.cpp
@@ -0,0 +1,249 @@
+#include "paramhandler.h"
+
+#define ERROR -1
+#define OK 0
+
+cParamHandler::cParamHandler(void) :
+	m_orb_set_param(-1),
+	m_orb_get_param(-1),
+
+	//TODO? Make target_* modifiable?
+	target_system(1),
+	target_component(50),
+
+	is_requested(false),
+	request_time(0),
+
+	load_request_fail_count(0),
+	max_load_request_fail_count(0),
+	load_update_timeout(100),
+	load_update_fail_count(0),
+	max_load_update_fail_count(0),
+
+	loaded_count(0),
+
+	upload_requred(false)
+{
+	get_param_s.param_index = -1;
+	get_param_s.target_system = target_system;
+	get_param_s.target_component = target_component;
+
+	set_param_s.target_system = target_system;
+	set_param_s.target_component = target_component;
+}
+
+cParamHandler::~cParamHandler(void) {
+	if(m_orb_get_param >= 0)
+		orb_unsubscribe(m_orb_get_param);
+	if(m_orb_set_param >= 0)
+		orb_unsubscribe(m_orb_set_param);
+}
+
+bool cParamHandler::init(void) {
+	passed_param_sub = orb_subscribe(ORB_ID(pass_drone_parameter));
+	return passed_param_sub != ERROR;
+}
+
+void cParamHandler::setupSave(orb_advert_t *orb_cmd, int32_t system_id, int32_t component_id) {
+	m_orb_cmd = orb_cmd;
+
+	memset(&save_cmd, 0, sizeof(save_cmd));
+	save_cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE;
+	save_cmd.param1 = 1;
+	save_cmd.confirmation = false;
+	save_cmd.source_system = system_id;
+	save_cmd.source_component = component_id;
+	save_cmd.target_system = target_system;
+	save_cmd.target_component = target_component;
+}
+
+void cParamHandler::loadCycle(void) {
+	if(is_requested) {
+		int res;
+		bool updated;
+
+		res = orb_check(passed_param_sub, &updated);
+		if(res == -1) {
+			//orb_check failed
+		} 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];
+
+			if(!strncmp(param.param_id, pparam->name, sizeof(param.param_id))) {
+				pparam->type = (enum param_type)param.param_type;
+				switch(pparam->type) {
+					case PTYPE_INT:
+						pparam->value = *(int*)&param.param_value;
+						break;
+					case PTYPE_FLOAT:
+						pparam->value = param.param_value;
+						break;
+				}
+				pparam->value *= pparam->mul;
+				pparam->editing_value = pparam->value;
+				loaded_count++;
+			} else {
+				//Must not happen ?
+			}
+
+			is_requested = false;
+		} else if((hrt_absolute_time() - request_time) / 10000.0f > load_update_timeout) {
+			load_update_fail_count++;
+			if(load_update_fail_count >= max_load_update_fail_count) {
+				//TODO: Do something?
+			}
+			is_requested = false;
+		}
+	} else if(loaded_count < getParamCount()) {
+		if(!request((enum param_id)loaded_count)) {
+			load_request_fail_count++;
+			if(load_request_fail_count >= max_load_request_fail_count) {
+				//TODO: Do something?
+			}
+		}
+	}
+}
+
+float cParamHandler::get(int id) {
+	return paramTable[id].value;
+}
+
+bool cParamHandler::request(enum param_id id) {
+	strncpy(get_param_s.param_id, paramTable[id].name, sizeof(get_param_s.param_id));
+
+	if (m_orb_get_param < 0) {
+		m_orb_get_param = orb_advertise(ORB_ID(get_drone_parameter), &get_param_s);
+		is_requested = m_orb_get_param != ERROR;
+	} else {
+		is_requested = orb_publish(ORB_ID(get_drone_parameter), m_orb_get_param, &get_param_s) == OK;
+	}
+
+	if(is_requested) {
+		request_time = hrt_absolute_time();
+	}
+
+	return is_requested;
+}
+
+bool cParamHandler::send(ad_param_t *param, bool upload) {
+	bool result;
+
+	strncpy(set_param_s.param_id, param->name, sizeof(set_param_s.param_id));
+	set_param_s.param_type = param->type;
+	set_param_s.param_value = convertValue(param->editing_value / param->mul, param->type);
+
+	if(m_orb_set_param < 0) {
+		m_orb_set_param = orb_advertise(ORB_ID(set_drone_parameter), &set_param_s);
+		result = m_orb_set_param != ERROR;
+	} else {
+		result = orb_publish(ORB_ID(set_drone_parameter), m_orb_set_param, &set_param_s) == OK;
+	}
+
+	if(result) {
+		param->value = param->editing_value;
+		upload_requred = true;
+	} else {
+		param->editing_value = param->value;
+		return false;
+	}
+
+	if(upload) {
+		return uploadAllParams();
+	}
+
+	return true;
+}
+
+bool cParamHandler::send(enum param_id id, float new_value, bool upload) {
+	ad_param_t *param = &paramTable[id];
+	param->editing_value = new_value;
+	return send(param, upload);
+}
+
+bool cParamHandler::sendCustomParam(const char *name, enum param_type type, float new_value, bool upload) {
+	bool result;
+
+	strncpy(set_param_s.param_id, name, sizeof(set_param_s.param_id));
+	set_param_s.param_type = type;
+	set_param_s.param_value = convertValue(new_value, type);
+
+	if(m_orb_set_param < 0) {
+		m_orb_set_param = orb_advertise(ORB_ID(set_drone_parameter), &set_param_s);
+		result = m_orb_set_param != ERROR;
+	} else {
+		result = orb_publish(ORB_ID(set_drone_parameter), m_orb_set_param, &set_param_s) == OK;
+	}
+
+	if(result) {
+		upload_requred = true;
+		if(upload)
+			uploadAllParams();
+	}
+
+	return result;
+}
+
+void cParamHandler::incParam(int param_id) {
+	ad_param_t *param = &paramTable[param_id];
+	param->editing_value += param->step;
+	if(param->editing_value > param->max)
+		param->editing_value = param->min;
+}
+
+void cParamHandler::decParam(int param_id) {
+	ad_param_t *param = &paramTable[param_id];
+	param->editing_value -= param->step;
+	if(param->editing_value < param->min)
+		param->editing_value = param->max;
+}
+
+bool cParamHandler::setParam(int param_id) {
+	ad_param_t *param = &paramTable[param_id];
+	if(param->value == param->editing_value)
+		return true;
+
+	if(!send(param))
+		return false;
+
+	return true;
+}
+
+void cParamHandler::resetParam(int param_id) {
+	ad_param_t *param = &paramTable[param_id];
+	param->editing_value = param->value;
+}
+
+bool cParamHandler::uploadAllParams(void) {
+	if(!upload_requred)
+		return true;
+
+	if(m_orb_cmd) {
+		bool result;
+		if(*m_orb_cmd < 0) {
+			*m_orb_cmd = orb_advertise(ORB_ID(vehicle_command), &save_cmd);
+			result = *m_orb_cmd != ERROR;
+		} else {
+			result = orb_publish(ORB_ID(vehicle_command), *m_orb_cmd, &save_cmd) == OK;
+		}
+		upload_requred = !result;
+		return result;
+	} else {
+		return false;
+	}
+}
+
+float cParamHandler::convertValue(float value, enum param_type type) {
+	switch(type) {
+		case PTYPE_INT: {
+			int int_value = (int)value;
+			return *(float*)&int_value;
+			break;
+		}
+		case PTYPE_FLOAT:
+		default:
+			return value;
+			break;
+	}
+}
diff --git a/src/modules/airdog/paramhandler.h b/src/modules/airdog/paramhandler.h
new file mode 100644
index 000000000..460e22f50
--- /dev/null
+++ b/src/modules/airdog/paramhandler.h
@@ -0,0 +1,107 @@
+#ifndef PARAMHANDLER_H
+#define PARAMHANDLER_H
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/set_drone_parameter.h>
+#include <uORB/topics/get_drone_parameter.h>
+#include <uORB/topics/pass_drone_parameter.h>
+#include <uORB/topics/vehicle_command.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
+#include "common.h"
+#include "paramlist.h"
+
+#define PARAM_REQUEST_DELAY 100
+
+enum param_type {
+	PTYPE_INT = 6,
+	PTYPE_FLOAT = 9,
+};
+
+typedef struct {
+	const char *name;
+	float value;
+	float editing_value;
+	float mul;
+	enum param_type type;
+	float min;
+	float max;
+	float step;
+	const char *display_name;
+} ad_param_t;
+
+
+class cParamHandler
+{
+public:
+	cParamHandler(void);
+	~cParamHandler(void);
+
+	bool init(void);
+	void setupSave(orb_advert_t *orb_cmd, int32_t system_id, int32_t component_id);
+
+	void loadCycle(void);
+
+	float get(int id);
+
+	bool request(enum param_id id);
+	bool send(ad_param_t *param, bool upload = false);
+	bool send(enum param_id id, float new_value, bool upload = false);
+	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 int getParamCount(void) { return PARAM_COUNT; }
+
+	void incParam(int param_id);
+	void decParam(int param_id);
+	bool setParam(int param_id);
+	void resetParam(int param_id);
+
+	inline const char *getDisplaySymbols(int param_id) { return paramTable[param_id].display_name; }
+	inline float getEditingValue(int param_id) { return paramTable[param_id].editing_value; }
+	inline enum param_type getEditingType(int param_id) { return paramTable[param_id].type; }
+
+	bool uploadAllParams(void);
+
+private:
+	float convertValue(float value, enum param_type type);
+
+	struct get_drone_param_s get_param_s;
+	struct set_drone_param_s set_param_s;
+	struct vehicle_command_s save_cmd;
+
+	orb_advert_t *m_orb_cmd;
+	orb_advert_t m_orb_set_param;
+	orb_advert_t m_orb_get_param;
+
+	int passed_param_sub;
+
+	const int target_system;
+	const int target_component;
+
+	bool is_requested;
+	uint64_t request_time;
+	int load_request_fail_count;
+	int max_load_request_fail_count;
+	float load_update_timeout;
+	int load_update_fail_count;
+	int max_load_update_fail_count;
+
+	int loaded_count;
+
+	bool upload_requred;
+
+	#define X(n, id, cv, ev, mul, type, min, max, step, dsym) { #id, cv, ev, mul, type, min, max, step, dsym },
+	ad_param_t paramTable[PARAM_COUNT] = { PARAM_LIST };
+	#undef X
+};
+
+#undef PARAM_LIST //after paramlist.h
+
+#endif // PARAMHANDLER_H
diff --git a/src/modules/airdog/paramlist.h b/src/modules/airdog/paramlist.h
new file mode 100644
index 000000000..727d3301b
--- /dev/null
+++ b/src/modules/airdog/paramlist.h
@@ -0,0 +1,43 @@
+/*
+ * Column 0:		Parameter name
+ * Parameter id
+ * cv				Current value
+ * ev				Editing value (for menu)
+ * mul				Multiplier (param is divied by this value on load, and multiplied on saving)
+ * Type
+ * Min
+ * Max
+ * Step
+ * Display symbols	(for menu)
+ */
+
+#define PARAM_LIST \
+	/*								Parameter id		cv ev mul		Type			Min		Max		Step	Display symbols */\
+	X(0,	/* AYAW */				MPC_YAW_OFF,		0, 0, 1.0f,		PTYPE_INT,		0,		1,		1,		"AYA" ) \
+	X(1,	/* PI */				NAV_TALT_USE,		0, 0, 1.0f,		PTYPE_INT,		0,		1,		1,		" P1" ) \
+	X(2,	/* BA */				NAV_TALT_RPT,		0, 0, 1.0f,		PTYPE_INT,		0,		1,		1,		" BA" ) \
+	X(3,	/* FF */				MPC_FW_FF,			0, 0, 1.0f,		PTYPE_FLOAT,	0.0f,	1.0f,	0.05f,	" FF" ) \
+	X(4,	/* LPF */				MPC_FW_LPF,			0, 0, 1.0f,		PTYPE_FLOAT,	0.0f,	10.0f,	0.05f,	"LPF" ) \
+	X(5,	/* MD */				ATT_MAG_DECL,		0, 0, 1.0f,		PTYPE_FLOAT,	-99.0f,	99.0f,	1.0f,	"DEC" ) \
+	X(6,	/* INAV_W_Z_GPS_P */	INAV_W_Z_GPS_P,		0, 0, 1000.0f,	PTYPE_FLOAT,	0.0f,	5.0f,	0.1f,	"6P5" ) \
+	X(7,	/* MPC_FW_ALT_OFF */	MPC_FW_ALT_OFF,		0, 0, 1.0f,		PTYPE_FLOAT,	-100.0f,100.0f,	1.0f,	"AL0" ) \
+	X(8,	/* MPC_FW_MAX_YAW */	MPC_FW_MAX_YAW,		0, 0, 1.0f,		PTYPE_FLOAT,	45.0f,	180.0f,	5.0f,	" YA" ) \
+	X(9,	/* MPC_FW_MIN_DIST */	MPC_FW_MIN_DIST,	0, 0, 1.0f,		PTYPE_FLOAT,	0.0f,	50.0f,	1.0f,	"D15" ) \
+	X(10,	/* MPC_THR_MIN */		MPC_THR_MIN,		0, 0, 1.0f,		PTYPE_FLOAT,	0.0f,	0.9f,	0.05f,	"THR" ) \
+	X(11,	/* MPC_TILTMAX_AIR */	MPC_TILTMAX_AIR,	0, 0, 1.0f,		PTYPE_FLOAT,	15.0f,	60.0f,	5.0f,	"T1A" ) \
+	X(12,	/* MPC_TILTMAX_LND */	MPC_TILTMAX_LND,	0, 0, 1.0f,		PTYPE_FLOAT,	10.0f,	45.0f,	5.0f,	"T1L" ) \
+	X(13,	/* MPC_Z_VEL_MAX */		MPC_Z_VEL_MAX,		0, 0, 1.0f,		PTYPE_FLOAT,	1.0f,	10.0f,	0.5f,	"2UE" ) \
+	X(14,	/* MPC_XY_VEL_MAX */	MPC_XY_VEL_MAX,		0, 0, 1.0f,		PTYPE_FLOAT,	1.0f,	50.0f,	1.0f,	"YUE" ) \
+	X(15,	/* NAV_ACCEPT_RAD */	NAV_ACCEPT_RAD,		0, 0, 1.0f,		PTYPE_FLOAT,	1.0f,	100.0f,	5.0f,	"RAD" ) \
+	X(16,	/* NAV_FOL_RTL_TO */	NAV_FOL_RTL_TO,		0, 0, 1.0f,		PTYPE_FLOAT,	5.0f,	50.0f,	1.0f,	"RT0" ) \
+	X(17,	/* NAV_TAKEOFF_ALT */	NAV_TAKEOFF_ALT,	0, 0, 1.0f,		PTYPE_FLOAT, 	3.0f,	50.0f,	1.0f,	"TAL" ) \
+	X(18,	/* NAV_RTL_ALT */		NAV_RTL_ALT,		0, 0, 1.0f,		PTYPE_FLOAT,	3.0f,	50.0f,	1.0f,	"RAL" ) \
+	X(19,	/* NAV_LAND_HOME */		NAV_LAND_HOME,		0, 0, 1.0f,		PTYPE_INT,		0,		1,		1,		"LAH" )
+
+#define PARAM_COUNT 20
+
+#define X(n, id, cv, ev, mul, type, min, max, step, dsym) PARAM_##id = n,
+enum param_id {
+	PARAM_LIST
+};
+#undef X
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 8578baec5..e92dab4bc 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -230,3 +230,21 @@ ORB_DEFINE(wind_estimate, struct wind_estimate_s);
 
 #include "topics/target_global_position.h"
 ORB_DEFINE(target_global_position, struct target_global_position_s);
+
+#include "topics/trainer_global_position.h"
+ORB_DEFINE(trainer_global_position, struct trainer_global_position_s);
+
+#include "topics/airdog_status.h"
+ORB_DEFINE(airdog_status, struct airdog_status_s);
+
+#include "topics/airdog_path_log.h"
+ORB_DEFINE(airdog_path_log, struct airdog_path_log_s);
+
+#include "topics/set_drone_parameter.h"
+ORB_DEFINE(set_drone_parameter, struct set_drone_param_s);
+
+#include "topics/get_drone_parameter.h"
+ORB_DEFINE(get_drone_parameter, struct get_drone_param_s);
+
+#include "topics/pass_drone_parameter.h"
+ORB_DEFINE(pass_drone_parameter, struct pass_drone_param_s);
diff --git a/src/modules/uORB/topics/airdog_path_log.h b/src/modules/uORB/topics/airdog_path_log.h
new file mode 100644
index 000000000..c96476675
--- /dev/null
+++ b/src/modules/uORB/topics/airdog_path_log.h
@@ -0,0 +1,20 @@
+/**
+ * @file airdog_path_log.h
+ * Definition of the logger trigger command uORB topic.
+ */
+
+#ifndef TOPIC_PATH_LOG_H_
+#define TOPIC_PATH_LOG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct airdog_path_log_s {
+	uint8_t start;
+    uint8_t stop;
+ }; /**< command sent to sdlog2 */
+
+/* register this as object request broker structure */
+ORB_DECLARE(airdog_path_log);
+
+#endif
diff --git a/src/modules/uORB/topics/airdog_status.h b/src/modules/uORB/topics/airdog_status.h
new file mode 100644
index 000000000..bfe8b2347
--- /dev/null
+++ b/src/modules/uORB/topics/airdog_status.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airdog_status.h
+ * Definition of the vehicle command uORB topic.
+ */
+
+#ifndef TOPIC_AIRDOG_STATUS_H_
+#define TOPIC_AIRDOG_STATUS_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct airdog_status_s {
+	uint8_t main_mode;	// px4_custom_mode.main_mode
+	uint8_t sub_mode;	// px4_custom_mode.sub_mode
+ 	uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
+ 	uint8_t system_status; ///< System status flag, see MAV_STATE ENUM
+ 	uint64_t timestamp;
+ 	uint8_t battery_remaining; // 0-100%
+	uint16_t discharged_mah;
+ }; /**< command sent to vehicle */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(airdog_status);
+
+
+
+#endif
diff --git a/src/modules/uORB/topics/get_drone_parameter.h b/src/modules/uORB/topics/get_drone_parameter.h
new file mode 100644
index 000000000..d46cb9090
--- /dev/null
+++ b/src/modules/uORB/topics/get_drone_parameter.h
@@ -0,0 +1,19 @@
+/**
+ * @file set_drone_parameter.h
+ * Definition of the set drone parameter command uORB topic.
+ */
+
+#ifndef TOPIC_GET_DRONE_PARAM_H_
+#define TOPIC_GET_DRONE_PARAM_H_
+
+struct get_drone_param_s {
+    int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
+    uint8_t target_system; ///< System ID
+    uint8_t target_component; ///< Component ID
+    char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string 
+};
+
+/* register this as object request broker structure */
+ORB_DECLARE(get_drone_parameter);
+
+#endif
diff --git a/src/modules/uORB/topics/pass_drone_parameter.h b/src/modules/uORB/topics/pass_drone_parameter.h
new file mode 100644
index 000000000..d7a416613
--- /dev/null
+++ b/src/modules/uORB/topics/pass_drone_parameter.h
@@ -0,0 +1,18 @@
+/**
+ * @file set_drone_parameter.h
+ * Definition of the set drone parameter command uORB topic.
+ */
+
+#ifndef TOPIC_PASS_DRONE_PARAM_H_
+#define TOPIC_PASS_DRONE_PARAM_H_
+
+struct pass_drone_param_s {
+    float param_value; ///< Onboard parameter value
+    char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
+    uint8_t param_type;
+};
+
+/* register this as object request broker structure */
+ORB_DECLARE(pass_drone_parameter);
+
+#endif 
diff --git a/src/modules/uORB/topics/set_drone_parameter.h b/src/modules/uORB/topics/set_drone_parameter.h
new file mode 100644
index 000000000..cc280aba1
--- /dev/null
+++ b/src/modules/uORB/topics/set_drone_parameter.h
@@ -0,0 +1,20 @@
+/**
+ * @file set_drone_parameter.h
+ * Definition of the set drone parameter command uORB topic.
+ */
+
+#ifndef TOPIC_SET_DRONE_PARAM_H_
+#define TOPIC_SET_DRONE_PARAM_H_
+
+struct set_drone_param_s {
+    float param_value; ///< Onboard parameter value
+    uint8_t target_system; ///< System ID
+    uint8_t target_component; ///< Component ID
+    char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
+    uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
+};
+
+/* register this as object request broker structure */
+ORB_DECLARE(set_drone_parameter);
+
+#endif
diff --git a/src/modules/uORB/topics/trainer_global_position.h b/src/modules/uORB/topics/trainer_global_position.h
new file mode 100644
index 000000000..6d40a24d0
--- /dev/null
+++ b/src/modules/uORB/topics/trainer_global_position.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file target_global_position.h
+ * Definition of the global fused WGS84 target position uORB topic.
+ */
+
+#ifndef TRAINER_GLOBAL_POSITION_T_H_
+#define TRAINER_GLOBAL_POSITION_T_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+* Target global position in WGS84.
+*/
+struct trainer_global_position_s {
+	uint64_t timestamp;	/**< time of this estimate, in microseconds since system start */
+	uint8_t sysid;		/**< sysid of target */
+	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
+	bool valid;			/**< true if position satisfies validity criteria of estimator */
+
+	double lat;			/**< Latitude in degrees							 	   */
+	double lon;			/**< Longitude in degrees							 	   */
+	float alt;			/**< Altitude in meters								 	   */
+	float vel_n; 		/**< Ground north velocity, m/s				 			   */
+	float vel_e;		/**< Ground east velocity, m/s							   */
+	float vel_d;		/**< Ground downside velocity, m/s						   */
+    float eph_m;        /**< Error position horizontal, m                          */
+    float epv_m;        /**< Error position vetical, m                             */
+	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(trainer_global_position);
+
+#endif