void use_rnb_data(uint8_t power)
{
uint8_t brightness_matrix[6][6];
pack_measurements_into_matrix(brightness_matrix);;
//print_brightness_matrix(brightness_matrix);
uint8_t emitter_total[6];
uint8_t sensor_total[6];
fill_S_and_T(brightness_matrix, sensor_total, emitter_total);
float bearing = get_bearing(sensor_total);
float heading = get_heading(emitter_total, bearing);
float initial_range = get_initial_range_guess(bearing, heading, sensor_total, emitter_total, brightness_matrix, power);
float range = range_estimate(brightness_matrix, initial_range, bearing, heading, power);
//TODO: Actually incorporate the ID #.
//rnb temp = {.range = range, .bearing = bearing, .heading = heading, .id_number = 0};
//printf("Bearing: %f | Heading: %f | Initial Range Guess: %f | Range: %f\r\n", rad_to_deg(bearing), rad_to_deg(heading), initial_range, range);
last_good_rnb.range = range;
last_good_rnb.bearing = bearing;
last_good_rnb.heading = heading;
last_good_rnb.brightness_matrix_ptr = brightness_matrix;
last_good_rnb.id_number = last_command_source_id;
rnb_updated=1;
}
void use_rnb_data(){
//uint32_t start = get_time();
uint8_t power = 255;
int16_t matrixSum = processBrightMeas();
float bearing, heading;
float error;
calculate_bearing_and_heading(&bearing, &heading);
float initial_range = get_initial_range_guess(bearing, heading, power);
if(initial_range!=0&&!isnanf(initial_range)){
float range = range_estimate(initial_range, bearing, heading, power);
if(!isnanf(range)){
if(range<2*DROPLET_RADIUS) range=5.0;
float fdR, fdB, fdH;
float conf = sqrtf(matrixSum);
fdB = bearing;
fdH = heading;
fdR = range;
rStep.f = 10*FD_INIT_STEP;
bStep.f = FD_INIT_STEP;
hStep.f = FD_INIT_STEP;
prevSgnEdR=0;
prevSgnEdB=0;
prevSgnEdH=0;
//print_brightMeas();
error = calculate_innovation(range, bearing, heading);
//printf("(RNB) ID: %04X \r\n\tBefore: % 5.1f, % 6.1f, % 6.1f\r\n", rnbCmdID, fdR, rad_to_deg(fdB), rad_to_deg(fdH));
//uint8_t i;
//start = get_time();
//uint8_t earlyAbort;
//float newR, newB, newH;
//for(i=0;i<15;i++){
//earlyAbort = finiteDifferenceStep(fdR, fdB, fdH, &newR, &newB, &newH);
////printf("\t\t% 5.1f, % 6.1f, % 6.1f | %6.4f, %6.4f\r\n", fdR, rad_to_deg(fdB), rad_to_deg(fdH), bStep.f, hStep.f);
//fdR = newR;
//fdB = newB;
//fdH = newH;
//if(earlyAbort) break;
//}
range = fdR;
bearing = fdB;
heading = fdH;
error = calculate_innovation(range, bearing, heading);
//printf("\t After: % 5.1f, % 6.1f, % 6.1f, %6.2f [%hu]\r\n", fdR, rad_to_deg(fdB), rad_to_deg(fdH), error>3.0 ? (conf/(10.0*error*error)) : (conf/(error*error)), i);
//printf("\tTook %lu ms.\r\n", get_time()-start);
conf = conf/(error*error);
if(error>3.0){
conf = conf/10.0; //Nerf the confidence hard if the calculated error was too high.
}
if(isnan(conf)){
conf = 0.01;
}
last_good_rnb.id_number = rnbCmdID;
last_good_rnb.range = range;
last_good_rnb.bearing = bearing;
last_good_rnb.heading = heading;
last_good_rnb.conf = conf;
rnb_updated=1;
}
}
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
rnbProcessingFlag=0;
}
}
