t_color shade(t_env *e, t_obj object, t_vector3d inter, t_vector3d ray) { t_color final_color; t_vector3d n; t_obj *light; int i; t_list *cursor; i = 0; cursor = e->list_spot; final_color = set_rgb(0, 0, 0); while (cursor) { i++; light = (t_obj *)cursor->content; light_rotate(e, &object, &inter, light); n = calcul_normal(&object, &inter); final_color = add_rgb(final_color, diffuse_color(light, n, &object)); final_color = add_rgb(final_color, specular_color(light, n, &object, ray)); cursor = cursor->next; } if (i > 0) final_color = set_rgb(final_color.r / i, final_color.g / i, final_color.b / i); valid_rgb(&final_color); return (final_color); }
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; } }
void next_frame(int frame) { int i = rand() % 300; if (rand() % 3) { set_rgb(pixels, i, 0, 0, 0); } else { set_rgb(pixels, i, rand(), rand(), rand()); } for (int s = 0; s < 10; s++) { put_pixels(s, pixels, 300); } }
static void window_redraw(Window *w, Graphics *g) { Rect r; Point p; int l, h; int length; char *buffer; assert(lines != NULL); /* Each wide char is encoded in at most 6 UTF8 characters. For the Base Plane, * 3 UTF8 characters per Unicode character is enough */ buffer = malloc((6 * NUM_COLUMNS + 1) * sizeof(char)); if (buffer != NULL) { r = get_window_area(w); set_rgb(g, rgb(240,240,240)); //??? attrib fill_rect(g, r); set_rgb(g, rgb(0,0,0)); //??? attrib set_font(g, font); set_text_direction(g, LR_TB); p = pt(0,0); h = font_height(font); for (l = 0; l < NUM_LINES; l++) { #if defined _UNICODE /* convert the line to UTF8 */ int c; char *ptr; for (c = 0, ptr = buffer; c < NUM_COLUMNS; c++) amx_UTF8Put(ptr, &ptr, 6, lines + l * NUM_COLUMNS + c); *ptr = '\0'; length = (int)(ptr - buffer); #else /* assume line is ASCII */ memcpy(buffer, lines + l * NUM_COLUMNS, NUM_COLUMNS); buffer[NUM_COLUMNS] = '\0'; length = NUM_COLUMNS; #endif /* draw the line */ draw_utf8(g, p, buffer, length); p.y += h; if (p.y > r.height) break; } /* if */ free(buffer); } /* if */ }
void RGBLED::setLEDColor(int ledcolor) { switch (ledcolor) { case LED_COLOR_OFF: // off set_rgb(0,0,0); break; case LED_COLOR_RED: // red set_rgb(255,0,0); break; case LED_COLOR_YELLOW: // yellow set_rgb(255,70,0); break; case LED_COLOR_PURPLE: // purple set_rgb(255,0,255); break; case LED_COLOR_GREEN: // green set_rgb(0,255,0); break; case LED_COLOR_BLUE: // blue set_rgb(0,0,255); break; case LED_COLOR_WHITE: // white set_rgb(255,255,255); break; case LED_COLOR_AMBER: // amber set_rgb(255,20,0); break; } }
//------------------------------------------------Funktionen fuer das setzen von werten----------------------------------------------------- int set() { static const char set_help[] PROGMEM = "Usage: s\n\rt[hh:mm:ss] set time\n\r"; if(uart_string[1] == 'h') { uart_send_pgm_string(set_help); } #if rgb == 1 if(uart_string[1] == 'c') set_rgb(); else #endif #if time == 1 if(uart_string[1] == 't') { stunde = ascii_to_char((char *) &uart_string[2]); minute = ascii_to_char((char *) &uart_string[5]); sekunde = ascii_to_char((char *) &uart_string[8]); } else #endif { uart_send_pgm_string(set_help); return -1; } return 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; }
void draw_ball(float crow, float ccol, float radius, byte red, byte green, byte blue) { for (int r = floor(crow) - radius - 1; r <= ceil(crow) + radius + 1; r++) { for (int c = floor(ccol) - radius - 1; c <= ceil(ccol) + radius + 1; c++) { if (r >= 0 && r < NUM_ROWS) { float d = sqrt((r - crow)*(r - crow) + (c - ccol)*(c - ccol)); int index = pixel_index(r, modulo(c, NUM_COLUMNS)); if (d < radius - 0.5) { set_rgb(pixels, index, red, green, blue); } else if (d < radius + 0.5) { float frac = d - (radius - 0.5); set_rgb(pixels, index, red*frac, green*frac, blue*frac); } } } } }
// Animate each twinkle. Pass the time in us between updates void *twinkle(void *env) { int *tmp = env; int led = *tmp; // Initial direction int i; int delay; int r, g, b, step = 10; #define MASK 0x3f r = (rand() % MASK)/step; g = (rand() % MASK)/step; b = (rand() % MASK)/step; delay = 10000 + rand() % 50000; for(i=1; i<=step; i++) { set_rgb(i*r, i*g, i*b, led, delay); } // Make 1 in 10 roll down the string if(rand()%100) { for(i=step; i>0; i--) { set_rgb(i*r, i*g, i*g, led, delay); } set_rgb(0, 0, 0, led, 0); } else if(rand()%25) { for(i=led-1; i>=0; i--) { set_rgb( 0, 0, 0, i+1, 0); set_rgb((i%step)*r, (i%step)*g, (i%step)*b, i , 2*delay); } } else { for(i=led; i<MAX_STRING_LEN; i++) { set_rgb( 0, 0, 0, i, 0); set_rgb((i%step)*r, (i%step)*g, (i%step)*b, i+1 , 20*delay); } } pthread_detach(pthread_self()); }
// update - updates led according to timed_updated. Should be called // at 50Hz void RGBLed::update() { // return immediately if not enabled if (!_healthy) { return; } update_colours(); set_rgb(_red_des, _green_des, _blue_des); }
void erase_ball(float crow, float ccol, float radius) { for (int r = floor(crow) - radius - 1; r <= ceil(crow) + radius + 1; r++) { for (int c = floor(ccol) - radius - 1; c <= ceil(ccol) + radius + 1; c++) { if (r >= 0 && r < NUM_ROWS) { int index = pixel_index(r, modulo(c, NUM_COLUMNS)); set_rgb(pixels, index, 0, 0, 0); } } } }
void parser_light(char **tab, t_env *e) { t_obj spot; if (check_tab_is_valid(tab) == 0) ft_exit("too few argument", e); ft_bzero(&spot, sizeof(t_obj)); spot.type = SPOT; spot.origin = new_vector(atof(tab[1]), atof(tab[2]), atof(tab[3])); spot.color = set_rgb(atoi(tab[4]), atoi(tab[5]), atoi(tab[6])); ft_lstadd(&(e->list_spot), ft_lstnew(&spot, sizeof(t_obj))); }
// update - updates led according to timed_updated. Should be called // at 50Hz void RGBLed::update() { // return immediately if not enabled if (!_healthy) { return; } if (!pNotify->_rgb_led_override) { update_colours(); set_rgb(_red_des, _green_des, _blue_des); } else { update_override(); } }
static void apply_lighting_to_dust(volume<glm::vec4, X, Y, Z>& nebula_dust, const volume<glm::vec3, X, Y, Z>& light_volume, const volume<glm::vec4, X, Y, Z>& dust_volume, const GLfloat intensity_multiplier) { for(size_t x = 0; x < X; ++x) for(size_t y = 0; y < Y; ++y) for(size_t z = 0; z < Z; ++z) { glm::uvec3 pos(x, y, z); glm::vec3 color_tmp = (light_volume[pos] * intensity_multiplier) * dust_volume[pos].rgb(); set_rgb(nebula_dust[pos], color_tmp); nebula_dust[pos].a = glm::clamp(dust_volume[pos].a, 0.0f, 1.0f); } }
static void scheduler_free(volatile Task_t* tgt) { if((tgt<task_storage_arr)||(tgt>(&(task_storage_arr[MAX_NUM_SCHEDULED_TASKS])))) { printf_P(PSTR("ERROR: In scheduler_free, tgt (%X) was outside valid Task* range.\r\n"),tgt); set_rgb(0,0,255); delay_ms(60000); } tgt->arg = 0; tgt->period = 0; (tgt->func).noarg_function = NULL; tgt->scheduled_time = 0; tgt->next = NULL; }
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::init() { int ret; ret = I2C::init(); if (ret != OK) { warnx("I2C init failed"); return ret; } stop_script(); set_rgb(0,0,0); return OK; }
void DycoLEDDriver::pattern_step() { int16_t red_diff, blue_diff, green_diff; uint16_t red, blue, green; uint32_t diff_time; uint32_t cur_time; struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); cur_time = 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (_sketch_start_time.tv_sec + (_sketch_start_time.tv_nsec*1.0e-9))); uint16_t step_size; uint8_t next_color,color; if(_res == 0){ return; //pattern not set } if(_step >= _step_cnt){ _step = 0; _iter = 0; } next_color = _pattern_color[(_step+1)%_step_cnt]; color = _pattern_color[_step]; step_size = _pattern_time[_step]/_res; diff_time = cur_time - _prev_time; if((diff_time > step_size) && (_iter < _res)){ _iter++; _prev_time = cur_time; } //calculate difference between two consecutive colors b/w which transition is taking place red_diff = preset_color[next_color][0]*_brightness[(_step+1)%_step_cnt] - preset_color[color][0]*_brightness[_step]; green_diff = preset_color[next_color][1]*_brightness[(_step+1)%_step_cnt] - preset_color[color][1]*_brightness[_step]; blue_diff = preset_color[next_color][2]*_brightness[(_step+1)%_step_cnt] - preset_color[color][2]*_brightness[_step]; //calculate rgb values at current stage/time red = preset_color[color][0]*_brightness[_step] + (red_diff*_iter)/_res; green = preset_color[color][1]*_brightness[_step] + (green_diff*_iter)/_res; blue = preset_color[color][2]*_brightness[_step] + (blue_diff*_iter)/_res; if(_iter >= _res){ _step++; //get ready for next step/transition _iter =0; } set_rgb(red,green,blue); }
void next_frame(int f) { if (f == 0) { for (int r = 0; r < NUM_ROWS; r++) { for (int c = 0; c < NUM_COLUMNS; c++) { set_rgb(pixels, pixel_index(r, c), (r+c)%2, (r+c)%2, 0); } } } float x = f; //sin(f/100.0)*50 + 60; float y = f; //sin(f/100.0)*50 + 60; draw_ball(x, y, 0.5, 240, 120, 0); for (int s = 0; s < NUM_SEGS; s++) { put_pixels(s, pixels + s*SEG_PIXELS*3, SEG_PIXELS); } erase_ball(x, y, 3); }
int save_bitmap(TTF_Bitmap *bitmap, const char *filename, const char *title) { FILE *fp = NULL; png_structp png_ptr = NULL; png_infop info_ptr = NULL; png_byte *row = NULL; RETINIT(SUCCESS); CHECKFAIL(bitmap, warn("failed to save uninitialized bitmap")); // Open file for writing (binary mode) fp = fopen(filename, "wb"); CHECKFAIL(fp, warnerr("failed to open file '%s' for saving bitmap", filename)); // Initialize the write structure png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); CHECKFAIL(png_ptr, warn("failed to alloc png write struct")); // Initialize the info structure info_ptr = png_create_info_struct(png_ptr); CHECKFAIL(info_ptr, warn("failed to alloc png info struct")); // Setup libpng exception handling CHECKFAIL(setjmp(png_jmpbuf(png_ptr)) == 0, warn("error occurred during png creation")); png_init_io(png_ptr, fp); // Write header (8 bit colour depth) png_set_IHDR(png_ptr, info_ptr, bitmap->w, bitmap->h, 8, PNG_COLOR_TYPE_RGB, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE); // Set png title if (title) { png_text png_title; png_title.compression = PNG_TEXT_COMPRESSION_NONE; png_title.key = (png_charp) "Title"; png_title.text = (png_charp) title; png_set_text(png_ptr, info_ptr, &png_title, 1); } png_write_info(png_ptr, info_ptr); // Allocate memory for one row (RBG = 3 bytes / pixel) row = (png_byte *) malloc((bitmap->w * 3) * sizeof(*row)); CHECKFAIL(row, warnerr("failed to alloc png row")); // Write image data int x, y; for (y = 0; y < bitmap->h; y++) { for (x = 0; x < bitmap->w; x++) { set_rgb(&(row[x*3]), bitmap->data[y*bitmap->w + x]); } png_write_row(png_ptr, (png_bytep)row); } // End write png_write_end(png_ptr, NULL); RETRELEASE( /* RELEASE */ if (row) free(row); if (info_ptr) png_free_data(png_ptr, info_ptr, PNG_FREE_ALL, -1); if (png_ptr) png_destroy_write_struct(&png_ptr, &info_ptr); if (fp) fclose(fp); );
void BlinkM::led() { static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; static int t_led_blink = 0; static int led_thread_runcount=0; static int led_interval = 1000; static int no_data_vehicle_status = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; static bool detected_cells_blinked = false; static bool led_thread_ready = true; int num_of_used_sats = 0; if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); 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; } t_led_color[5] = LED_OFF; 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_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); 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_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; } /* get number of used satellites in navigation */ num_of_used_sats = 0; //for(int satloop=0; satloop<20; satloop++) { for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } if(new_data_vehicle_status || no_data_vehicle_status < 3){ if(num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; } printf("<blinkm> cells found:%u\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { /* 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 if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* 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 { /* no battery warnings here */ if(vehicle_status_raw.flag_system_armed == false) { /* system not armed */ 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_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; /* handle 4th led - flightmode indicator */ switch((int)vehicle_status_raw.flight_mode) { case VEHICLE_FLIGHT_MODE_MANUAL: led_color_4 = LED_AMBER; break; case VEHICLE_FLIGHT_MODE_STAB: led_color_4 = LED_YELLOW; break; case VEHICLE_FLIGHT_MODE_HOLD: led_color_4 = LED_BLUE; break; case VEHICLE_FLIGHT_MODE_AUTO: led_color_4 = LED_GREEN; break; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat´s */ 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( "<blinkm> 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 DycoLEDDriver::set_solid_color(uint8_t color) { set_rgb(preset_color[color][0],preset_color[color][1],preset_color[color][2]); _res = 0; }
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("<blinkm> 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("<blinkm> 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( "<blinkm> 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 program_step(void) { struct rgb_colour colour_rgb; struct hsv_colour colour_hsv; uint16_t param = 0; switch((enum program_opcode) read_byte()) { case OP_HALT: instruction--; return; case OP_SET_RGB: colour_rgb.red = read_byte(); colour_rgb.green = read_byte(); colour_rgb.blue = read_byte(); set_rgb(colour_rgb); // immediatly execute next step - since program_step(); break; case OP_SET_HSV: colour_hsv.hue = read_byte(); colour_hsv.saturation = read_byte(); colour_hsv.value = read_byte(); set_hsv(colour_hsv); // immediatly execute next step - since program_step(); break; case OP_FADE_RGB: colour_rgb.red = read_byte(); colour_rgb.green = read_byte(); colour_rgb.blue = read_byte(); // Duration param |= (read_byte() << 8) | read_byte(); fade_rgb(colour_rgb, param); break; case OP_FADE_HSV: colour_hsv.hue = read_byte(); colour_hsv.saturation = read_byte(); colour_hsv.value = read_byte(); // Duration param |= (read_byte() << 8) | read_byte(); fade_hsv(colour_hsv, param); break; case OP_WAIT: // Duration param |= (read_byte() << 8) | read_byte(); wait(param); break; case OP_GOTO_ROM: // Target offset param |= (read_byte() << 8) | read_byte(); current_addrspace = ADDR_ROM; instruction = rom + param; program_step(); break; case OP_GOTO: // Target offset param |= (read_byte() << 8) | read_byte(); if(current_addrspace == ADDR_ROM) { instruction = rom + param; } else { instruction = ram_base + param; } program_step(); break; case OP_WRITE_ROM: // read offset at which to store new values param |= (read_byte() << 8) | read_byte(); uint8_t *write_pos = rom + param; // Write n bytes for(param = read_byte(); param > 0; param--) { if(write_pos < rom || write_pos >= (rom + rom_size)) break; eeprom_write_byte(write_pos++, read_byte()); } program_step(); break; default: break; } }
void IR_emit(uint8_t direction, uint8_t duration) // this is now BLOCKING *** { // as of now, this routine: // 1. turns on a pink color for 200 ms // 2. turns off pink, turns on green color while it blasts IR for 100 ms // 3. turns off green, and returns // note that the IR blast occurs during the latter 1/3 of the routine // somewhere in there it toggles some pins to temporarily disable carrier wave and UART // note further that the brightness is set elsewhere in the code // note: IR carrier pins { PIN0_bm, PIN1_bm, PIN4_bm, PIN5_bm, PIN7_bm, PIN6_bm }; uint8_t USART_CTRLB_save; uint8_t carrier_wave_bm; uint8_t TX_pin_bm; PORT_t * the_UART_port; USART_t * the_USART; switch(direction) { case 0: // WORKS! carrier_wave_bm = PIN0_bm; // TODO, name these "PIN0_bm" to something more specific, like "IR_CARRIER_PIN_0" TX_pin_bm = PIN3_bm; the_UART_port = &PORTC; the_USART = &USARTC0; break; case 1: // WORKS! carrier_wave_bm = PIN1_bm; TX_pin_bm = PIN7_bm; the_UART_port = &PORTC; the_USART = &USARTC1; break; case 2: // WORKS! carrier_wave_bm = PIN4_bm; TX_pin_bm = PIN3_bm; the_UART_port = &PORTD; the_USART = &USARTD0; break; case 3: // WORKS! carrier_wave_bm = PIN5_bm; TX_pin_bm = PIN3_bm; the_UART_port = &PORTE; the_USART = &USARTE0; break; case 4: // WORKS! carrier_wave_bm = PIN7_bm; TX_pin_bm = PIN7_bm; the_UART_port = &PORTE; the_USART = &USARTE1; break; case 5: // WORKS! carrier_wave_bm = PIN6_bm; TX_pin_bm = PIN3_bm; the_UART_port = &PORTF; the_USART = &USARTF0; break; default: break; } USART_CTRLB_save = the_USART->CTRLB; // record the current state of the USART TCF2.CTRLB &= ~carrier_wave_bm; // disable carrier wave output PORTF.DIRSET = carrier_wave_bm; // enable user output on this pin PORTF.OUT |= carrier_wave_bm; // high signal on this pin the_USART->CTRLB = 0; // disable USART the_UART_port->DIRSET = TX_pin_bm; // enable user output on this pin the_UART_port->OUT &= ~TX_pin_bm; // low signal on TX pin (IR LED is ON when this pin is LOW, these pins were inverted in software during initialization) // IR LIGHT IS ON NOW //_delay_ms(NUMBER_OF_RB_MEASUREMENTS * DELAY_BETWEEN_RB_MEASUREMENTS); // -1 only counting gaps between N measurements // 0.1 delay while taking a meas //_delay_ms((NUMBER_OF_RB_MEASUREMENTS-1) * (DELAY_BETWEEN_RB_MEASUREMENTS+0.1)*0.75); delay_ms(duration); // IR LIGHT IS about to go OFF the_UART_port->OUT |= TX_pin_bm; // high signal on TX pin (turns IR blast OFF) the_USART->CTRLB = USART_CTRLB_save; // re-enable USART (restore settings as it was before) PORTF.OUT &= ~carrier_wave_bm; // low signal on the carrier wave pin, don't really know why we do this? probably not necessary TCF2.CTRLB |= carrier_wave_bm; // re-enable carrier wave output set_rgb(0,0,0); }
void program_step(void) { struct rgb_colour colour_rgb; struct hsv_colour colour_hsv; uint16_t duration = 0; switch((enum program_opcode) read_byte()) { case OP_HALT: instruction--; return; case OP_SET_RGB: colour_rgb.red = read_byte(); colour_rgb.green = read_byte(); colour_rgb.blue = read_byte(); set_rgb(colour_rgb); // immediatly execute next step - since program_step(); break; case OP_SET_HSV: colour_hsv.hue = read_byte(); colour_hsv.saturation = read_byte(); colour_hsv.value = read_byte(); set_hsv(colour_hsv); // immediatly execute next step - since program_step(); break; case OP_FADE_RGB: colour_rgb.red = read_byte(); colour_rgb.green = read_byte(); colour_rgb.blue = read_byte(); duration |= (read_byte() << 8) | read_byte(); fade_rgb(colour_rgb, duration); break; case OP_FADE_HSV: colour_hsv.hue = read_byte(); colour_hsv.saturation = read_byte(); colour_hsv.value = read_byte(); duration |= (read_byte() << 8) | read_byte(); fade_hsv(colour_hsv, duration); break; case OP_WAIT: duration |= (read_byte() << 8) | read_byte(); wait(duration); break; case OP_GOTO_ROM: current_addrspace = ADDR_ROM; case OP_GOTO: duration |= (read_byte() << 8) | read_byte(); if(current_addrspace == ADDR_ROM) { instruction = rom + duration; } else { instruction = ram_base + duration; } program_step(); break; default: break; } }