/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * 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 blinkm.cpp * * Driver for the BlinkM LED controller connected via I2C. * * Connect the BlinkM to I2C3 and put the following line to the rc startup-script: * blinkm start * * To start the system monitor put in the next line after the blinkm start: * blinkm systemmonitor * * * Description: * After startup, the Application checked how many lipo cells are connected to the System. * The recognized number off cells, will be blinked 5 times in purple color. * 2 Cells = 2 blinks * ... * 6 Cells = 6 blinks * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. * * System disarmed and safe: * The BlinkM should light solid cyan. * * System safety off but not armed: * The BlinkM should light flashing orange * * System armed: * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. * * X-X-X-X-_-_-_-_-_-_- * ------------------------- * G G G M * P P P O * S S S D * E * * (X = on, _=off) * * The first 3 blinks indicates the status of the GPS-Signal (red): * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- * 5 satellites = X-X-_-X-X-_-_-_-_-_- * 6 satellites = X-_-_-X-X-_-_-_-_-_- * >=7 satellites = _-_-_-X-X-_-_-_-_-_- * If no GPS is found the first 3 blinks are white * * The fourth Blink indicates the Flightmode: * MANUAL : amber * STABILIZED : yellow * HOLD : blue * AUTO : green * * Battery Warning (low Battery Level): * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X * * Battery Alert (critical Battery Level) * Continuously blinking in red X-X-X-X-X-X-X-X-X-X * * General Error (no uOrb Data) * Continuously blinking in white X-X-X-X-X-X-X-X-X-X * */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include static const float MAX_CELL_VOLTAGE = 4.3f; static const int LED_ONTIME = 120; static const int LED_OFFTIME = 120; static const int LED_BLINK = 1; static const int LED_NOBLINK = 0; class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); virtual ~BlinkM(); virtual int init(); virtual int probe(); virtual int setMode(int mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); static const char *const script_names[]; private: enum ScriptID { USER = 0, RGB, WHITE_FLASH, RED_FLASH, GREEN_FLASH, BLUE_FLASH, CYAN_FLASH, MAGENTA_FLASH, YELLOW_FLASH, BLACK, HUE_CYCLE, MOOD_LIGHT, VIRTUAL_CANDLE, WATER_REFLECTIONS, OLD_NEON, THE_SEASONS, THUNDERSTORM, STOP_LIGHT, MORSE_CODE }; enum ledColors { LED_OFF, LED_RED, LED_ORANGE, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, LED_CYAN, LED_WHITE, LED_AMBER }; work_s _work; int led_color_1; int led_color_2; int led_color_3; int led_color_4; int led_color_5; int led_color_6; int led_color_7; int led_color_8; int led_blink; bool systemstate_run; int vehicle_status_sub_fd; int vehicle_control_mode_sub_fd; int vehicle_gps_position_sub_fd; int actuator_armed_sub_fd; int safety_sub_fd; int num_of_cells; int detected_cells_runcount; int t_led_color[8]; int t_led_blink; int led_thread_runcount; int led_interval; bool topic_initialized; bool detected_cells_blinked; bool led_thread_ready; int num_of_used_sats; void setLEDColor(int ledcolor); static void led_trampoline(void *arg); void led(); int set_rgb(uint8_t r, uint8_t g, uint8_t b); int fade_rgb(uint8_t r, uint8_t g, uint8_t b); int fade_hsb(uint8_t h, uint8_t s, uint8_t b); int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); int set_fade_speed(uint8_t s); int play_script(uint8_t script_id); int play_script(const char *script_name); int stop_script(); int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); int set_script(uint8_t length, uint8_t repeats); int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); int get_firmware_version(uint8_t version[2]); }; /* for now, we only support one BlinkM */ namespace { BlinkM *g_blinkm; } /* list of script names, must match script ID numbers */ const char *const BlinkM::script_names[] = { "USER", "RGB", "WHITE_FLASH", "RED_FLASH", "GREEN_FLASH", "BLUE_FLASH", "CYAN_FLASH", "MAGENTA_FLASH", "YELLOW_FLASH", "BLACK", "HUE_CYCLE", "MOOD_LIGHT", "VIRTUAL_CANDLE", "WATER_REFLECTIONS", "OLD_NEON", "THE_SEASONS", "THUNDERSTORM", "STOP_LIGHT", "MORSE_CODE", nullptr }; extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm, 100000), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), led_color_4(LED_OFF), led_color_5(LED_OFF), led_color_6(LED_OFF), led_color_7(LED_OFF), led_color_8(LED_OFF), led_blink(LED_NOBLINK), systemstate_run(false), vehicle_status_sub_fd(-1), vehicle_control_mode_sub_fd(-1), vehicle_gps_position_sub_fd(-1), actuator_armed_sub_fd(-1), safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), topic_initialized(false), detected_cells_blinked(false), led_thread_ready(true), num_of_used_sats(0) { memset(&_work, 0, sizeof(_work)); } BlinkM::~BlinkM() { } int BlinkM::init() { int ret; ret = I2C::init(); if (ret != OK) { warnx("I2C init failed"); return ret; } stop_script(); set_rgb(0,0,0); return OK; } int BlinkM::setMode(int mode) { if(mode == 1) { if(systemstate_run == false) { stop_script(); set_rgb(0,0,0); systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } } else { systemstate_run = false; } return OK; } int BlinkM::probe() { uint8_t version[2]; int ret; ret = get_firmware_version(version); if (ret == OK) log("found BlinkM firmware version %c%c", version[1], version[0]); return ret; } int BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; switch (cmd) { case BLINKM_PLAY_SCRIPT_NAMED: if (arg == 0) { ret = EINVAL; break; } ret = play_script((const char *)arg); break; case BLINKM_PLAY_SCRIPT: ret = play_script(arg); break; case BLINKM_SET_USER_SCRIPT: { if (arg == 0) { ret = EINVAL; break; } unsigned lines = 0; const uint8_t *script = (const uint8_t *)arg; while ((lines < 50) && (script[1] != 0)) { ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); if (ret != OK) break; script += 5; } if (ret == OK) ret = set_script(lines, 0); break; } default: break; } return ret; } void BlinkM::led_trampoline(void *arg) { BlinkM *bm = (BlinkM *)arg; bm->led(); } void BlinkM::led() { if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 250); vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); orb_set_interval(vehicle_control_mode_sub_fd, 250); actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(actuator_armed_sub_fd, 250); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 250); /* Subscribe to safety topic */ safety_sub_fd = orb_subscribe(ORB_ID(safety)); orb_set_interval(safety_sub_fd, 250); topic_initialized = true; } if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { t_led_color[0] = LED_PURPLE; } if(num_of_cells > 1) { t_led_color[1] = LED_PURPLE; } if(num_of_cells > 2) { t_led_color[2] = LED_PURPLE; } if(num_of_cells > 3) { t_led_color[3] = LED_PURPLE; } if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } if(num_of_cells > 5) { t_led_color[5] = LED_PURPLE; } t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; } else { t_led_color[0] = led_color_1; t_led_color[1] = led_color_2; t_led_color[2] = led_color_3; t_led_color[3] = led_color_4; t_led_color[4] = led_color_5; t_led_color[5] = led_color_6; t_led_color[6] = led_color_7; t_led_color[7] = led_color_8; t_led_blink = led_blink; } led_thread_ready = false; } if (led_thread_runcount & 1) { if (t_led_blink) setLEDColor(LED_OFF); led_interval = LED_OFFTIME; } else { setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); //led_interval = (led_thread_runcount & 1) : LED_ONTIME; led_interval = LED_ONTIME; } if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; struct vehicle_control_mode_s vehicle_control_mode; struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; struct safety_s safety; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); memset(&safety, 0, sizeof(safety)); bool new_data_vehicle_status; bool new_data_vehicle_control_mode; bool new_data_actuator_armed; bool new_data_vehicle_gps_position; bool new_data_safety; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); int no_data_vehicle_status = 0; int no_data_vehicle_control_mode = 0; int no_data_actuator_armed = 0; int no_data_vehicle_gps_position = 0; if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; } else { no_data_vehicle_status++; if(no_data_vehicle_status >= 3) no_data_vehicle_status = 3; } orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); if (new_data_vehicle_control_mode) { orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); no_data_vehicle_control_mode = 0; } else { no_data_vehicle_control_mode++; if(no_data_vehicle_control_mode >= 3) no_data_vehicle_control_mode = 3; } orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); if (new_data_actuator_armed) { orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); no_data_actuator_armed = 0; } else { no_data_actuator_armed++; if(no_data_actuator_armed >= 3) no_data_actuator_armed = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); no_data_vehicle_gps_position = 0; } else { no_data_vehicle_gps_position++; if(no_data_vehicle_gps_position >= 3) no_data_vehicle_gps_position = 3; } /* update safety topic */ orb_check(safety_sub_fd, &new_data_safety); if (new_data_safety) { orb_copy(ORB_ID(safety), safety_sub_fd, &safety); } /* get number of used satellites in navigation */ num_of_used_sats = vehicle_gps_position_raw.satellites_used; if (new_data_vehicle_status || no_data_vehicle_status < 3) { if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf(" checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf(" cells found:%d\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_RED; led_color_5 = LED_RED; led_color_6 = LED_RED; led_color_7 = LED_RED; led_color_8 = LED_RED; led_blink = LED_BLINK; } else if(vehicle_status_raw.rc_signal_lost) { /* LED Pattern for FAILSAFE */ led_color_1 = LED_BLUE; led_color_2 = LED_BLUE; led_color_3 = LED_BLUE; led_color_4 = LED_BLUE; led_color_5 = LED_BLUE; led_color_6 = LED_BLUE; led_color_7 = LED_BLUE; led_color_8 = LED_BLUE; led_blink = LED_BLINK; } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; led_color_3 = LED_YELLOW; led_color_4 = LED_YELLOW; led_color_5 = LED_YELLOW; led_color_6 = LED_YELLOW; led_color_7 = LED_YELLOW; led_color_8 = LED_YELLOW; led_blink = LED_BLINK; } else { /* no battery warnings here */ if(actuator_armed.armed == false) { /* system not armed */ if(safety.safety_off){ led_color_1 = LED_ORANGE; led_color_2 = LED_ORANGE; led_color_3 = LED_ORANGE; led_color_4 = LED_ORANGE; led_color_5 = LED_ORANGE; led_color_6 = LED_ORANGE; led_color_7 = LED_ORANGE; led_color_8 = LED_ORANGE; led_blink = LED_BLINK; }else{ led_color_1 = LED_CYAN; led_color_2 = LED_CYAN; led_color_3 = LED_CYAN; led_color_4 = LED_CYAN; led_color_5 = LED_CYAN; led_color_6 = LED_CYAN; led_color_7 = LED_CYAN; led_color_8 = LED_CYAN; led_blink = LED_NOBLINK; } } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_OFF; led_color_5 = LED_OFF; led_color_6 = LED_OFF; led_color_7 = LED_OFF; led_color_8 = LED_OFF; led_blink = LED_BLINK; if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; /* TODO: add other Auto modes */ else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; led_color_5 = led_color_4; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used satus */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 6) { led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 5) { led_color_3 = LED_OFF; } } else { /* no vehicle_gps_position data */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; } } } } } else { /* LED Pattern for general Error - no vehicle_status can retrieved */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; led_color_4 = LED_WHITE; led_color_5 = LED_WHITE; led_color_6 = LED_WHITE; led_color_7 = LED_WHITE; led_color_8 = LED_WHITE; led_blink = LED_BLINK; } /* printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", vehicle_status_raw.voltage_battery, vehicle_status_raw.flag_system_armed, vehicle_status_raw.flight_mode, num_of_cells, no_data_vehicle_status, no_data_vehicle_gps_position, num_of_used_sats, vehicle_gps_position_raw.fix_type, vehicle_gps_position_raw.satellites_visible); */ led_thread_runcount=0; led_thread_ready = true; led_interval = LED_OFFTIME; if(detected_cells_runcount < 4){ detected_cells_runcount++; } else { detected_cells_blinked = true; } } else { led_thread_runcount++; } if(systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); } else { stop_script(); set_rgb(0,0,0); } } void BlinkM::setLEDColor(int ledcolor) { switch (ledcolor) { case LED_OFF: // off set_rgb(0,0,0); break; case LED_RED: // red set_rgb(255,0,0); break; case LED_ORANGE: // orange set_rgb(255,150,0); break; case LED_YELLOW: // yellow set_rgb(200,200,0); break; case LED_PURPLE: // purple set_rgb(255,0,255); break; case LED_GREEN: // green set_rgb(0,255,0); break; case LED_BLUE: // blue set_rgb(0,0,255); break; case LED_CYAN: // cyan set_rgb(0,128,128); break; case LED_WHITE: // white set_rgb(255,255,255); break; case LED_AMBER: // amber set_rgb(255,65,0); break; } } int BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) { const uint8_t msg[4] = { 'n', r, g, b }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) { const uint8_t msg[4] = { 'c', r, g, b }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) { const uint8_t msg[4] = { 'h', h, s, b }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) { const uint8_t msg[4] = { 'C', r, g, b }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) { const uint8_t msg[4] = { 'H', h, s, b }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::set_fade_speed(uint8_t s) { const uint8_t msg[2] = { 'f', s }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::play_script(uint8_t script_id) { const uint8_t msg[4] = { 'p', script_id, 0, 0 }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::play_script(const char *script_name) { /* handle HTML colour encoding */ if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { char code[3]; uint8_t r, g, b; code[2] = '\0'; code[0] = script_name[1]; code[1] = script_name[2]; r = strtol(code, 0, 16); code[0] = script_name[3]; code[1] = script_name[4]; g = strtol(code, 0, 16); code[0] = script_name[5]; code[1] = script_name[6]; b = strtol(code, 0, 16); stop_script(); return set_rgb(r, g, b); } for (unsigned i = 0; script_names[i] != nullptr; i++) if (!strcasecmp(script_name, script_names[i])) return play_script(i); return -1; } int BlinkM::stop_script() { const uint8_t msg[1] = { 'o' }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) { const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) { const uint8_t msg[3] = { 'R', 0, line }; uint8_t result[5]; int ret = transfer(msg, sizeof(msg), result, sizeof(result)); if (ret == OK) { ticks = result[0]; cmd[0] = result[1]; cmd[1] = result[2]; cmd[2] = result[3]; cmd[3] = result[4]; } return ret; } int BlinkM::set_script(uint8_t len, uint8_t repeats) { const uint8_t msg[4] = { 'L', 0, len, repeats }; return transfer(msg, sizeof(msg), nullptr, 0); } int BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) { const uint8_t msg = 'g'; uint8_t result[3]; int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); if (ret == OK) { r = result[0]; g = result[1]; b = result[2]; } return ret; } int BlinkM::get_firmware_version(uint8_t version[2]) { const uint8_t msg = 'Z'; return transfer(&msg, sizeof(msg), version, 2); } void blinkm_usage(); void blinkm_usage() { warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); warnx("options:"); warnx("\t-b --bus i2cbus (3)"); warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int blinkm_main(int argc, char *argv[]) { int i2cdevice = PX4_I2C_BUS_EXPANSION; int blinkmadr = 9; int x; for (x = 1; x < argc; x++) { if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { if (argc > x + 1) { i2cdevice = atoi(argv[x + 1]); } } if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) { if (argc > x + 1) { blinkmadr = atoi(argv[x + 1]); } } } if (!strcmp(argv[1], "start")) { if (g_blinkm != nullptr) errx(1, "already started"); g_blinkm = new BlinkM(i2cdevice, blinkmadr); if (g_blinkm == nullptr) errx(1, "new failed"); if (OK != g_blinkm->init()) { delete g_blinkm; g_blinkm = nullptr; errx(1, "init failed"); } exit(0); } if (g_blinkm == nullptr) { fprintf(stderr, "not started\n"); blinkm_usage(); exit(0); } if (!strcmp(argv[1], "systemstate")) { g_blinkm->setMode(1); exit(0); } if (!strcmp(argv[1], "ledoff")) { g_blinkm->setMode(0); exit(0); } if (!strcmp(argv[1], "list")) { for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) fprintf(stderr, " %s\n", BlinkM::script_names[i]); fprintf(stderr, " \n"); exit(0); } /* things that require access to the device */ int fd = open(BLINKM0_DEVICE_PATH, 0); if (fd < 0) err(1, "can't open BlinkM device"); g_blinkm->setMode(0); if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) exit(0); blinkm_usage(); exit(0); }