Revision 1443 trunk/code/projects/libdragonfly/bom.c

View differences:

bom.c
226 226
        return lowest_i;
227 227
    else
228 228
        return -1;
229
}
230

  
231
/** 
232
 * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.
233
 *
234
 * @pre must call bom refresh
235
 * @param dist  pointer to int in which to return the estimated distance to the other robot
236
 * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the
237
 *        index of the max bom value.  -1 if no value is lower than BOM_VALUE_THRESHOLD.
238
 **/
239
int bom_get_max10(int *dist) {
240
    int i, max;
241
    long long mean, sum;
242

  
243
    max = bom_get_max();
244
    if (max < 0)
245
    {
246
        if (dist)
247
        {
248
            *dist = -1;
249
        }
250
        return -1;
251
    }
252
    /* Record values into an array */
253
    for (i = 0; i < NUM_BOM_LEDS; i++) {
254
        int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max);
255
        int val = 255 - bom_val[i];
256
        mean += idx * val;
257
        sum += val;
258
    }
259
    mean = (mean * 10) / sum;
260
    mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10);
261

  
262
    if (dist)
263
    {
264
        *dist = 50 - sum/48;
265
    }
266

  
267
    return mean;
229 268
}
230 269

  
231 270
/**

Also available in: Unified diff