예제 #1
0
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;
}
예제 #2
0
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;
	}
}