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;
}
}
