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