float get_initial_range_guess(float bearing, float heading, uint8_t power){
int8_t bestS = (6-((int8_t)ceilf((3.0*bearing)/M_PI)))%6;
float alpha = pretty_angle(bearing - basis_angle[bestS]); //alpha using infinite approximation
int8_t bestE = (6-((int8_t)ceilf((3.0*(bearing-heading-M_PI))/M_PI)))%6;
float beta = pretty_angle(bearing - heading - basis_angle[bestE] - M_PI); //beta using infinite approximation
//printf("(alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
if((alpha > M_PI_2) || (alpha < -M_PI_2)){
printf("ERROR: alpha out of range (alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
return 0;
}
if((beta > M_PI_2) || (beta < -M_PI_2)){
printf("ERROR: beta out of range (beta: %f, emitter %u)\r\n", beta, bestE);
return 0;
}
//printf("(beta: %f, emitter %u)\r\n", rad_to_deg(beta), bestE);
// expected contribution (using infinite distance approximation)
float amplitude;
float exp_con = sensor_model(alpha)*emitter_model(beta);
if(exp_con > 0) amplitude = brightMeas[bestE][bestS]/exp_con;
else{
printf("ERROR: exp_con (%f) is negative (or zero)!\r\n", exp_con);
return 0;
}
//printf("amp_for_inv: %f\t",amplitude);
float rMagEst = inverse_amplitude_model(amplitude, power);
float RX = rMagEst*cos(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][0]-headingBasis[bestE][0]);
float RY = rMagEst*sin(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][1]-headingBasis[bestE][1]);
float rangeEst = hypotf(RX,RY);
return rangeEst;
}
float get_heading(uint8_t emitter_total[6], float bearing)
{
volatile float x_sum = 0.0;
volatile float y_sum = 0.0;
volatile float bearing_according_to_TX;
for(uint8_t i = 0; i < 6; i++)
{
x_sum = x_sum + basis[i][0]*emitter_total[i];
y_sum = y_sum + basis[i][1]*emitter_total[i];
}
bearing_according_to_TX = atan2f(y_sum, x_sum);
float heading = bearing + M_PI - bearing_according_to_TX;
return pretty_angle(heading);
}
float deg_to_rad(float deg){
return pretty_angle( (deg / 180) * M_PI );
}
float rad_to_deg(float rad){
return (pretty_angle(rad) / M_PI) * 180;
}
float range_estimate(uint8_t brightness_matrix[6][6], float range_upper_limit, float bearing, float heading, uint8_t power)
{
float alpha, beta;
float SEcontribution;
float sensorRXx, sensorRXy, sensorTXx, sensorTXy;
float calcRIJmag, calcRmag;
float calcRx, calcRy;
float range_matrix[6][6];
uint8_t maxBright = 0;
uint8_t maxE;
uint8_t maxS;
for(uint8_t e = 0; e < 6; e++)
{
for(uint8_t s = 0; s < 6; s++)
{
if(brightness_matrix[e][s]>maxBright)
{
maxBright = brightness_matrix[e][s];
maxE = e;
maxS = s;
}
if(brightness_matrix[e][s] > BASELINE_NOISE_THRESHOLD)
{
sensorRXx = DROPLET_SENSOR_RADIUS*basis[s][0];
sensorRXy = DROPLET_SENSOR_RADIUS*basis[s][1];
sensorTXx = DROPLET_SENSOR_RADIUS*basis[e][0] + range_upper_limit*cosf(bearing);
sensorTXy = DROPLET_SENSOR_RADIUS*basis[e][1] + range_upper_limit*sinf(bearing);
alpha = atan2f(sensorTXy-sensorRXy,sensorTXx-sensorRXx) - basis_angle[s];
beta = atan2f(sensorRXy-sensorTXy,sensorRXx-sensorTXx) - basis_angle[e] - heading;
alpha = pretty_angle(alpha);
beta = pretty_angle(beta);
//if((alpha < M_PI/2)&&(alpha > -M_PI/2))
//{
//if((beta < M_PI/2)&&(beta > -M_PI/2))
//{
SEcontribution = sensor_model(alpha)*emitter_model(beta);
calcRIJmag = inverse_amplitude_model(brightness_matrix[e][s]/SEcontribution, power);
calcRx = calcRIJmag*cosf(alpha) + DROPLET_SENSOR_RADIUS*(basis[s][0] - basis[e][0]);
calcRy = calcRIJmag*sinf(alpha) + DROPLET_SENSOR_RADIUS*(basis[s][1] - basis[e][1]);
range_matrix[e][s] = sqrt(calcRx*calcRx + calcRy*calcRy);
continue;
//}
//}
}
range_matrix[e][s]=0;
}
}
//
//for(uint8_t e=0; e<6 ; e++){
//for(uint8_t s=0; s<6; s++){
//printf("%02.3f ",range_matrix[e][s]);
//}
//printf("\r\n");
//}
float range = range_matrix[maxE][maxS];
//printf("range from 0,0: %f\r\n",range_matrix[maxE][maxS]);
return range;
}
float get_initial_range_guess(float bearing, float heading, uint8_t sensor_total[6], uint8_t emitter_total[6], uint8_t brightness_matrix[6][6], uint8_t power)
{
uint8_t i, e, s, best_e, best_s;
uint16_t biggest_e_val = 0;
uint16_t biggest_s_val = 0;
float alpha, beta;
float EC, amplitude;
float inital_guess;
for(i = 0; i < 6; i++)
{
if(emitter_total[i] > biggest_e_val)
{
best_e = i;
biggest_e_val = emitter_total[best_e];
}
if(sensor_total[i] > biggest_s_val)
{
best_s = i;
biggest_s_val = sensor_total[best_s];
}
}
/*printf("BEST SENSOR: %u\r\n", best_s);
printf("BEST EMITTER: %u\r\n", best_e);*/
// find alpha using infinite approximation
alpha = bearing - atan2f(basis[best_s][1],basis[best_s][0]); // TODO: dont use atan2 for this simple task
alpha = pretty_angle(alpha);
// find beta using infinite approximation
beta = bearing - heading - atan2f(basis[best_e][1],basis[best_e][0]) - M_PI; // TODO: dont use atan2 for this simple task
beta = pretty_angle(beta);
// expected contribution (using infinite distance approximation)
if((alpha > M_PI/2)||(alpha < -M_PI/2)) // pi/2 = 1.5708
{
printf("ERROR: alpha out of range (alpha: %f, sensor %u)\r\n",alpha, best_s);
}
if((beta > M_PI/2)||(beta < -M_PI/2))
{
printf("ERROR: beta out of range (beta: %f, emitter %u)\r\n", beta, best_e);
}
EC = sensor_model(alpha)*emitter_model(beta);
if(EC > 0)
{
amplitude = brightness_matrix[best_e][best_s]/EC;
}
else
{
printf("ERROR: EC is negative (or zero)! oh, my (EC = %f)\r\n", EC);
}
return inverse_amplitude_model(amplitude, power) + 2*DROPLET_RADIUS;
}
float range_estimate(float init_range, float bearing, float heading, uint8_t power){
float range_matrix[6][6];
float sensorRXx, sensorRXy, sensorTXx, sensorTXy;
float alpha, beta, sense_emit_contr;
float calcRIJmag, calcRx, calcRy;
int16_t maxBright = -32768;
uint8_t maxE=0;
uint8_t maxS=0;
for(uint8_t e = 0; e < 6; e++){
for(uint8_t s = 0; s < 6; s++){
if(brightMeas[e][s]>maxBright){
maxBright = brightMeas[e][s];
maxE = e;
maxS = s;
}
if(brightMeas[e][s] > 0){
sensorRXx = DROPLET_RADIUS*getCosBearingBasis(0,s);
sensorRXy = DROPLET_RADIUS*getSinBearingBasis(0,s);
sensorTXx = DROPLET_RADIUS*cosf(basis_angle[e]+heading) + init_range*cosf(bearing);
sensorTXy = DROPLET_RADIUS*sinf(basis_angle[e]+heading) + init_range*sinf(bearing);
alpha = atan2f(sensorTXy-sensorRXy,sensorTXx-sensorRXx) - basis_angle[s];
beta = atan2f(sensorRXy-sensorTXy,sensorRXx-sensorTXx) - basis_angle[e] - heading;
alpha = pretty_angle(alpha);
beta = pretty_angle(beta);
sense_emit_contr = sensor_model(alpha)*emitter_model(beta);
//printf("sense_emit_contr: %f\r\n",sense_emit_contr);
if(sense_emit_contr>0){
calcRIJmag = inverse_amplitude_model(brightMeas[e][s]/sense_emit_contr, power);
}else{
calcRIJmag = 0;
}
calcRx = calcRIJmag*cosf(alpha) + sensorRXx - DROPLET_RADIUS*cosf(basis_angle[e]+heading);
calcRy = calcRIJmag*sinf(alpha) + sensorRXy - DROPLET_RADIUS*sinf(basis_angle[e]+heading);
range_matrix[e][s] = hypotf(calcRx, calcRy);
continue;
}
range_matrix[e][s]=0;
}
}
float rangeMatSubset[3][3];
float brightMatSubset[3][3];
float froebNormSquared=0;
for(uint8_t e = 0; e < 3; e++){
for(uint8_t s = 0; s < 3; s++){
uint8_t otherE = ((maxE+(e+5))%6);
uint8_t otherS = ((maxS+(s+5))%6);
rangeMatSubset[e][s] = range_matrix[otherE][otherS];
brightMatSubset[e][s] = (float)brightMeas[otherE][otherS];
froebNormSquared+=powf(brightMatSubset[e][s],2);
}
}
float froebNorm = sqrtf(froebNormSquared);
float range = 0;
for(uint8_t e = 0; e < 3; e++){
for(uint8_t s = 0; s < 3; s++){
range+= rangeMatSubset[e][s]*powf(brightMatSubset[e][s]/froebNorm,2);
}
}
//printf("R: %f\r\n", range);
//print_range_matrix(range_matrix);
//printf("\n");
return range;
}