File content as of revision 8:5590f55bdf41:

#include "mbed.h"
#include "edge_sensor.h"
#include "VEML6040.h"
#include "edge_color.h"

/* VEML6040 config bits */
/* sensor config loser 4bit */
/* trigger mode etc. */
#define SD_BIT   0x01
#define AF_BIT   0x02
#define TRIG_BIT 0x04

/* sensor config upper 4bit */
/* integration time */
 int sensor_delay[] = { 
    40,
    80,
    160,
    320,
    640,
    1280, 
    1280, /* place holder */ 
    1280  /* place holder */
} ;
uint16_t        color0_pwm[3] ;
uint16_t        color1_pwm[3] ;
uint16_t        color0_target[3] = { 3000, 3000, 3000 } ;
uint16_t        color1_target[3] = { 3000, 3000, 3000 } ;

edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor() 
{
    uint16_t dummy[3] ;
    _sensor = sensor ;
    _sensor_config = AF_BIT | TRIG_BIT ;
    _interval = 30 ;

    _pwm_period = 2000 ; /* 2ms */
    _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
//    _probe = 0xFF00 ; 
    for (int i = 0 ; i < 3 ; i++ ) {
        _led[i] = led[i] ;
        _led[i]->write(1.0) ; /* turn LED off */
        _value[i] = 0 ;
        _pwm[i] = pwm[i] ;
        _led[i]->period_us(_pwm_period) ;
    }
    getRGB(dummy) ; // dummy read, the first data is usually garbage
}

edge_color::~edge_color(void) 
{
    delete _sensor ;
    delete [] _led ;
}

void edge_color::setLEDs(uint16_t led_value[]) 
{
    for (int i = 0 ; i < 3 ; i++ ) {
        _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
    }
}

void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b) 
{
    _led[0]->write((float)(65535 - r)/65535.0) ;
    _led[1]->write((float)(65535 - g)/65535.0) ;
    _led[2]->write((float)(65535 - b)/65535.0) ;
}

void edge_color::reset(void) 
{
    for (int i = 0 ; i < 3 ; i++ ) {
        _value[i] = 0 ;
    }
}

void edge_color::prepare(void) 
{
//    setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
}

void edge_color::sample(void) 
{
    setLEDs(_pwm) ;
    getRGB(_value) ;
#if 0
    _sensor->setCOLORConf( _sensor_config ) ;
    wait_ms(sensor_delay[(_sensor_config >> 4)&0x07]*1.25) ;
    _sensor->getRData(&_value[0]) ;
    _sensor->getGData(&_value[1]) ;
    _sensor->getBData(&_value[2]) ;
#endif
    _sampled_time = edge_time ;
    setLEDs(0, 0, 0) ; /* turn LEDs off */
}

int edge_color::deliver(void) 
{
    int result ;
    char timestr[16] ;
    print_time(_sampled_time) ;
    time2seq(_sampled_time, timestr) ;
    printf(" color %d : R = %4d, G = %4d, B = %4d\n",
        _id, _value[0], _value[1], _value[2]) ;
    if (_id == 1) { /* color1 */
    sprintf(_str_buf,
          "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
          _value[0], _value[1], _value[2], timestr, _error_count) ; 
    } else { /* color2 */
        sprintf(_str_buf,
          "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
          _value[0], _value[1], _value[2], timestr, _error_count) ; 
    }
    result = afero->setAttribute(1, _str_buf) ;
    return( result == afSUCCESS ) ;
}

void edge_color::getRGB(uint16_t v[])
{
    _sensor->setCOLORConf(_sensor_config) ;

    wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
    
    _sensor->getRData(&v[0]) ;
    _sensor->getGData(&v[1]) ;
    _sensor->getBData(&v[2]) ;
}

/**
 * Measure num_ave + 2 times
 * and throw away min and max
 * before calculating average
 */ 
void edge_color::getAveColor(uint16_t v[], int num_ave)
{
    int i, c ;
    long l[3] = {0, 0, 0} ;
    uint16_t min[3] = { 0, 0, 0 } ;
    uint16_t max[3] = { 0, 0, 0 } ;
    uint16_t *tmp ;
    
    tmp = new uint16_t[(num_ave+2)*3] ;
    
    getRGB(&tmp[0]) ; // dummy read
    wait(0.1) ;
    for (i = 0 ; i < num_ave+2 ; i++ ) {
        getRGB(&tmp[i*3]) ;
// printf("%04x %04x %04x\n", tmp[i].r, tmp[i].g, tmp[i].b) ;
        for (c = 0 ; c < 3 ; c++ ) { /* c = r, g, b */
            if (tmp[i*3+c] < tmp[min[c]]) {
                min[c] = i ;
            }
            if (tmp[i*3+c] > tmp[max[c]]) {
                max[c] = i ;
            }
        }
    }
    for (i = 0 ; i < num_ave+2 ; i++ ) {
        for (c = 0 ; c < 3 ; c++ ) {
            if ((min[c] != i)&&(max[c] != i)) {
                l[c] += tmp[i*3+c] ;
            }
        }
    }

    delete [] tmp ;
    for (c = 0 ; c < 3 ; c++ ) {
        v[c] = (uint16_t)(l[c] / num_ave) ;
    }
// printf("=== average ===\n") ;
// printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
}

void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
{
   const uint16_t led_interval = 10 ; /* wait 10ms for LED */
    double      denominator ;
    double      numerator[3] ;
    double      a,b,c,d,e,f,g,h,i ;
    uint16_t    v[3], tmp[3] ;
//    uint16_t    L[3][3] ;
    double    L[3][3] ;
    double      ftarget[3] ;
    int         idx ;
    
    ftarget[0] = target[0] ;
    ftarget[1] = target[1] ;
    ftarget[2] = target[2] ;
    printf("=== Calibrating Color Sensor %d ===\n", _id) ;
    for (idx = 0 ; idx < 3 ; idx++ ) {
        tmp[0] = tmp[1] = tmp[2] = 0 ;
        tmp[idx] = _probe ;

        setLEDs(tmp) ;
        wait_ms(led_interval) ;
        getAveColor(v, num_ave) ;
        
        printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
        L[idx][0] = v[0] ;
        L[idx][1] = v[1] ;
        L[idx][2] = v[2] ;
        setLEDs(0, 0, 0) ; /* clear LEDs */
    }
    
    printf("=== Initial Equation ===\n") ;
    for (idx = 0 ; idx < 3 ; idx++) {
        printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
        (int)L[0][idx], _probe, (int)L[1][idx], _probe,  (int)L[2][idx], _probe, target[idx]) ;
    }

     a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
     d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
     g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
     
    denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;

    if (denominator != 0) {
        numerator[0] = (f * h - e * i) * ftarget[0] 
            + b * (i * ftarget[1] - f * ftarget[2]) 
            + c * (e * ftarget[2] - h * ftarget[1]) ;

        numerator[1] = -((f * g - d * i) * ftarget[0]
            + a * (i * ftarget[1] - f * ftarget[2]) 
            + c * (d * ftarget[2] - g * ftarget[1])) ;

        numerator[2] = (e * g - d * h) * ftarget[0]
            + a * (h * ftarget[1] - e * ftarget[2]) 
            + b * (d * ftarget[2] - g * ftarget[1]) ;

        for (idx = 0 ; idx < 3 ; idx++ ) {
            _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
            result[idx] = _pwm[idx] ;
        }

        printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
        printf("G = %d [0x%04x] ", result[1], result[1]) ;
        printf("B = %d [0x%04x] ", result[2], result[2]) ;
        printf("\n") ;
        printf("=== test ===\n") ;
        setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
        wait_ms(led_interval) ;
        getAveColor(v, num_ave) ;
        printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
        printf("============\n") ;
    } else {
        printf("calibration failed, pwm values were not updated\n") ;
    }
}