void rc_init ( void ) { fp_dev = fopen ("FP_DEV_NAME", "r"); if (fp_dev) { led_log ("Opened FP_DEV_NAME\n"); rc_system_status = STATUS_INITIALIZED; // Should we be doing this? } else { led_log ("Failed to open FP_DEV_NAME\n"); rc_system_status = STATUS_FAIL; } }
/* actuators are arranged in 4 banks of 16. Because of how access is done in the oboard API these banks are in the ranges 16-32, 32-48, 64-80 and 128-144 so we convert that into a single number 0-64 so we can use a single array for storage */ static uint8_t actuator_id_to_index(ActuatorID_t id) { uint8_t base; uint8_t offset = (id & 0x0F); switch (id & 0xF0) { case ACTUATOR_BANK_MOTORS: base = 0 * ACTUATOR_MAX; break; case ACTUATOR_BANK_SERVOS: base = 1 * ACTUATOR_MAX; break; case ACTUATOR_BANK_3: base = 2 * ACTUATOR_MAX; break; case ACTUATOR_BANK_4: base = 4 * ACTUATOR_MAX; break; default: led_log("Unknown actuator bank 0x%X\n", id & 0xF0); return 0; } return base + offset; }
/* BRIEF: led control message handler * INPUT: led control message string * NOTE: must be no sapce in the input message. * return: * true, if message format is correct and set led, * false, if message format is incorrect and do nothing to led. * * message string format: [switch,][hues,][saturation,][brightness] * switch[0/1] * hues[0-360] * saturation[0-100] * brightness[0-100] * eg (red): 1, 0, 100, 100 * | | | | * / | | \ * switch hues saturation brightness * string length = 11 (no sapce) */ bool LedControlMsgHandler(unsigned char *Msg, unsigned int len) { bool ret = false; float *color_t = NULL; unsigned char *ptr = NULL; unsigned char led_hues_str[4] = {0}; unsigned char led_saturation_str[4] = {0}; unsigned char led_brightness_str[4] = {0}; int led_hues_value = 0; int led_saturation_value = 0; int led_brightness_value = 0; char delims[] = ","; char *psubstr = NULL; //null msg if ((0 == len) || (NULL == Msg)){ return false; } switch(Msg[0]) { case '0':{ // led off if(1 != len) ret = false; else{ led_log("CloseLED."); CloseLED_RGB(); ret = true; } break; } case '1':{ // led on if (len < 7) // strlen("1,0,0,0") ret = false; else{ // get msg data ptr = (unsigned char*)malloc(len-1); memset(ptr, 0, len-1); memcpy(ptr, Msg+2, len-2); //led_log("ptr[%d]=%s", strlen((char *)ptr), ptr); // get HSB data string psubstr = strtok((char*)ptr, delims); if ((psubstr != NULL) && (strlen(psubstr)<=3)) { memcpy(led_hues_str, psubstr, strlen(psubstr)); //led_log("led_hues_str[%d]=%s", strlen((char*)led_hues_str), led_hues_str); psubstr = strtok( NULL, delims ); if ((psubstr != NULL) && (strlen(psubstr)<=3)) { memcpy(led_saturation_str, psubstr, strlen(psubstr)); //led_log("led_saturation_str[%d]=%s", strlen((char*)led_saturation_str), led_saturation_str); psubstr = strtok( NULL, delims ); if ((psubstr != NULL) && (strlen(psubstr)<=3)) { memcpy(led_brightness_str, psubstr, strlen(psubstr)); //led_log("led_brightness_str[%d]=%s", strlen((char*)led_brightness_str), led_brightness_str); //get HSB string ok, transform to int and set LED if(Str2Int(led_hues_str, &led_hues_value) && \ Str2Int(led_saturation_str, &led_saturation_value) \ && Str2Int(led_brightness_str, &led_brightness_value)){ color_t = (float*)malloc(3); if (NULL != color_t){ led_log("OpenLED: H=%d, S=%d, B=%d", led_hues_value, led_saturation_value, led_brightness_value); H2R_HSBtoRGB((float)led_hues_value, (float)led_saturation_value, (float)led_brightness_value, color_t); OpenLED_RGB(color_t); free(color_t); color_t = NULL; ret = true; } } } } } if(NULL != ptr){ free(ptr); ptr = NULL; } } break; } default: ret = false; break; } return ret; }