diff options
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(¶m_bat_warn, 0, sizeof(param_bat_warn)); + memset(¶m_bat_failsafe, 0, sizeof(param_bat_failsafe)); + memset(¶m_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, ¶m); + pparam = ¶mTable[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*)¶m.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 = ¶mTable[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 = ¶mTable[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 = ¶mTable[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 = ¶mTable[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 = ¶mTable[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 |