Commit 1b048f05 authored by Andrey Filippov's avatar Andrey Filippov

Debugging multi-satelite search

parent d49ee8a4
......@@ -7,7 +7,7 @@ class PolynomialApproximation
public $debugLevel = 1;
public $debugFile = null;
public function polynomialApproximation1d($data, $N){
public function polynomialApproximation1d($data, $N, $noreduce=true){
//$my_array = array_fill(0, $size_of_the_array, $some_data);
if ($this->debugFile === null){
$this->debugLevel = 0;
......@@ -70,8 +70,14 @@ class PolynomialApproximation
return $result;
} catch (Exception $e) {
echo 'Caught exception: ', $e->getMessage(), ", reducing polynomial order\n";
if ($N1 < 0 ) {
//// fprintf($dbg_file, print_r(debug_backtrace(),1));
fprintf($this->debugFile,
"polynomialApproximation1d() Caught exception: ".$e->getMessage(), ", reducing polynomial order\n");
fprintf($this->debugFile, print_r(debug_backtrace(),1)."\n");
fprintf($this->debugFile, "data=".print_r(debug_backtrace(),1)."\nMatrix M:\n");
$M->print($this->debugFile);
// echo 'Caught exception: ', $e->getMessage(), ", reducing polynomial order\n";
if (($N1 < 0) || $noreduce) {
return null;
}
$N1--;
......
......@@ -19,7 +19,7 @@ $AZ_CMAX = 4790;
$EL_CMAX = 10979;
$SK_CMAX = 931;
$MODEM_THRESHOLD = 17.0; // consider something if modem strength is above
$OVERSHOOT = 1; // final steps absolute error
$OVERSHOOT = 2; // 1; // final steps absolute error
$LATE_STOP_AZ = 30; // steps
// Rescan azimuth after noticed a satellite during area scan
......@@ -69,7 +69,7 @@ http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop
//php > var_dump($DishAngle);
//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}
$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test05.log","w");
$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test07.log","w");
fprintf($dbg_file,"test log\n");
if (false) {
......@@ -141,25 +141,23 @@ if (!$scan_mode){
}
if ($full_search){
$rslt = lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step);
$rslt_sat = lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step);
if ($dbg_file !== null) {
fprintf($dbg_file, "lockToSatellite --> %s\n", print_r($rslt_sat,1));
}
$xml = new SimpleXMLElement("<?xml version='1.0'?><motosat/>");
$State = $xml_state->State+0; // 1 - Ready, 2 - Busy
$State = $xml_state->State+0; // 1 - Ready, 2 - Busy, 3 - Stow! (turned to stow by no reason?)
$busy = ($State & 2) != 0;
$SignalQuality = myfval(explode(",",$xml_state->SignalQuality)[0]);
// print result to XML
// $xml->addChild ('Success',(string) $Success );
$xml->addChild ('SignalQuality',(string) $SignalQuality );
// if (isset($DishAngle1D)){
// $xml->addChild ('DishAngle1D',$DishAngle1D );
// }
// if (isset($DishAngle2D)){
// $xml->addChild ('DishAngle2D',$DishAngle2D );
// }
$xml->addChild ('DishAngle',$xml_state->DishAngle );
$xml->addChild ('DishCount',$xml_state->DishCountt );
$xml->addChild ('busy',(string) $busy );
$xml->addChild ('State',(string) $State );
$xml->addChild ('Result',(string) print_r($rslt_sat,1));
$rslt=$xml->asXML();
header("Content-Type: text/xml");
......@@ -252,9 +250,14 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
$DishAngleCenter[0] = $DishAngle[0];
$DishAngleCenter[1] = $DishAngle[1];
$empty_az = array(); // array of pairs: ('low'=>min, "high"=>max) for scanned empoty azimuths in degrees
$satellites =array();// array of ('az'=>az, "el"=> el, 'strength'-> strength, 'guard_low', 'guard_high') in degrees for each foubnd satellite
$satellites =array();// array of ('az'=>az, "el"=> el, 'strength'-> strength, 'guard_low', 'guard_high') in degrees for each found satellite
$num_az_steps = (int) ceil ($wnd_az/$SAT_AZ_STEP);
$az_min = $DishAngle[0] - $wnd_az/2;
if ($dbg_file !== null) {
fprintf($dbg_file, "+++++ lockToSatellite( %f, %s, %f, %f, %f\n",
$min_strength, print_r($DishAngle,1), $wnd_az, $wnd_el, $scan_step);
}
// $az_max = $az_min + ($num_az_steps)* $SAT_AZ_STEP;
while (true) { // loop until found correct satellite, or failed
$az_array = array_fill(0, $num_az_steps + 1, 0);
......@@ -269,20 +272,37 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
}
}
if ($dbg_file !== null) {
fprintf($dbg_file, "0. az_array= %s\n", str_replace("\n"," ",print_r($az_array,1))."\n");
}
// Exclude each satellite with its guard range
foreach ($satellites as $indx => $azels){
$i_min = (int) round (($azels['az'] - $azels['guard_low'])/$SAT_AZ_STEP);
$i_max = (int) round (($azels['az'] + $azels['guard_high'])/$SAT_AZ_STEP);
$i_min = (int) round (($azels['az'] - $az_min - $azels['guard_low'])/$SAT_AZ_STEP);
$i_max = (int) round (($azels['az'] - $az_min + $azels['guard_high'])/$SAT_AZ_STEP);
if ($dbg_file !== null) {
fprintf($dbg_file, "i_min = %d, i_max= %d\n", $i_min, $i_max);
}
if ($i_min < 0) $i_min = 0;
if ($i_max > $num_az_steps) $i_max = $num_az_steps;
for ($i = $i_min; $i <= $i_max; $i++){
$az_array[$i] = $indx +1; // number of satellite, starting from 1
if ($dbg_file !== null) {
fprintf($dbg_file, "%d: %d\n", $i,$az_array[$i]);
}
}
}
if ($dbg_file !== null) {
fprintf($dbg_file, "1. az_array= %s\n", str_replace("\n"," ",print_r($az_array,1))."\n");
}
// Find azimuth range to scan
for ($i_low = 0; ($i_low <= $num_az_steps) && ($az_array[$i_low] == 0); $i_low++ );
for ($i_low = 0; ($i_low <= $num_az_steps) && ($az_array[$i_low] != 0); $i_low++ );
if ($i_low > $num_az_steps) { // all searched, no luck
// Here is te exit from thye loop !
// Here is te exit from the loop on failure!
return array ('satellite' => null, 'satellites' => $satellites, 'empty' => $empty_az);
}
for ($i_high = $i_low; ($i_high < $num_az_steps) && ($az_array[$i_high+1] == 0); $i_high++);
......@@ -294,27 +314,46 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
$high_end_sat = ($i_high < $num_az_steps)? $az_array[$i_high + 1] : -1;
$az_range = $az_high - $az_low;
$az_center = ($az_high + $az_low) / 2;
if ($dbg_file !== null) {
fprintf($dbg_file, "DishAngleCenter=".str_replace("\n"," ",print_r($DishAngleCenter,1))."\n");
fprintf($dbg_file, "low_end_sat = %d, high_end_sat= %d\n",$low_end_sat,$high_end_sat);
fprintf($dbg_file, "Satellites=".str_replace("\n"," ",print_r($satellites,1))."\n");
}
if ($low_end_sat < 0){
if ($high_end_sat < 0) { // both ends unknown
$el_center = $DishAngleCenter[1];
$el_range = $wnd_el;
if ($dbg_file !== null) {
fprintf($dbg_file, "1: el_center = %f, el_range= %f\n",$el_center,$el_range);
}
} else { // satellite on the right only
$el_center = $satellites[$high_end_sat -1]['el'];
$el_range = $MAX_ARC_TILT * $az_range;
if ($dbg_file !== null) {
fprintf($dbg_file, "2: el_center = %f, el_range= %f\n",$el_center,$el_range);
}
}
} else {
if ($high_end_sat < 0) {// satellite on the left only
$el_center = $satellites[$low_end_sat -1]['el'];
$el_range = $MAX_ARC_TILT * $az_range;
if ($dbg_file !== null) {
fprintf($dbg_file, "3: el_center = %f, el_range= %f\n",$el_center,$el_range);
}
} else { // both ends have satellites
$el_center = ($satellites[$high_end_sat -1]['el'] + $satellites[$low_end_sat -1]['el']) / 2 ;
$el_range = abs($satellites[$high_end_sat -1]['el'] - $satellites[$low_end_sat -1]['el']);
if ($dbg_file !== null) {
fprintf($dbg_file, "4: el_center = %f, el_range= %f\n",$el_center,$el_range);
}
}
// $xml_state = simplexml_load_file($motosat_url);
// $DishAnglesNow = getDishAngles ($xml_state); // will go there if failed
// $dir = ($DishAnglesNow[0] < $az_center) ? 1 : 0;
}
if ($el_center < 50){
return array ('satellite' => null, 'satellites' => $satellites, 'empty' => $empty_az);
}
// Do scanning of the range
if ($dbg_file !== null) {
fprintf($dbg_file, "==== Scanning sky window : center (az:%f7.3 el:%f7.3) az_range=%f7.3, el_range = %f7.3 ====\n",
......@@ -339,7 +378,15 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
// Did found some satellite - peak azimuth
// peakAzimuthOrElevation - limit range to scan range, do not allow max to be outside of the scan range!
$minmax = array($az_low, $az_high);
if ($dbg_file !== null) {
fprintf($dbg_file, "Starting peakAzimuthOrElevation(...(%f, %f))\n",$az_low,$az_high);
}
$rslt_az = peakAzimuthOrElevation($xml_state, 0, $AZ_PLAY, $AZ_RESCAN, $AZ_RESCAN_STEP, $USABLE_FRACT, $USABLE_POW, $ARGMAX_OURSIDE, $minmax);
if ($dbg_file !== null) {
fprintf($dbg_file, "peakAzimuthOrElevation(...(%f, %f)) returned: %s\n",$az_low,$az_high, (string) $rslt_az);
}
$success_az = ($rslt_az ===1) ? 1 : 0;
$xml_state = simplexml_load_file($motosat_url);
......@@ -356,7 +403,7 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
if (($az_sat - $sat['az']) < $AZ_TOL + $AZ_RESCAN_STEP){ // accepting larger tolerance
$satellites[$indx]['guard_high'] += $GUARD_STEP; // search range was on the right, increasing high guard range for the satellite
if ($dbg_file !== null) {
fprintf($dbg_file, "**** Increasing high guard range of the satellite :\n".print_r($satellites[$indx]));
fprintf($dbg_file, "**** Increasing high guard range of the satellite :\n".print_r($satellites[$indx],1));
}
continue;
}
......@@ -367,7 +414,7 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
if (($sat['az'] - $az_sat) < $AZ_TOL + $AZ_RESCAN_STEP){ // accepting larger tolerance
$satellites[$indx]['guard_low'] += $GUARD_STEP; // search range was on the left, increasing low guard range for the satellite
if ($dbg_file !== null) {
fprintf($dbg_file, "**** Increasing low guard range of the satellite :\n".print_r($satellites[$indx]));
fprintf($dbg_file, "**** Increasing low guard range of the satellite :\n".print_r($satellites[$indx],1));
}
continue;
}
......@@ -387,13 +434,13 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
if ($az_center < $sat['az']) {
$satellites[$indx]['guard_low'] += $GUARD_STEP; // search range was on the left, increasing low guard range for the satellite
if ($dbg_file !== null) {
fprintf($dbg_file, "**** Increasing low guard range of the satellite:\n".print_r($satellites[$indx]));
fprintf($dbg_file, "**** Increasing low guard range of the satellite:\n".print_r($satellites[$indx],1));
}
} else {
$satellites[$indx]['guard_high'] += $GUARD_STEP;// search range was on the right, increasing high guard range for the satellite
if ($dbg_file !== null) {
fprintf($dbg_file, "**** Increasing high guard range of the satellite:\n".print_r($satellites[$indx]));
fprintf($dbg_file, "**** Increasing high guard range of the satellite:\n".print_r($satellites[$indx],1));
}
}
continue; // recalculate azimuth range, try again
......@@ -410,7 +457,7 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
}
$modem_strength = getModemStrength();
$xml_state = simplexml_load_file($motosat_url);
$DishAngleSat = $xml_state->DishAngle;
$DishAngleSat = getDishAngles($xml_state); // ->DishAngle;
$satellite = array(
'az' => $DishAngleSat[0],
"el" => $DishAngleSat[1],
......@@ -448,7 +495,7 @@ function lockToSatellite($min_strength, $DishAngle, $wnd_az, $wnd_el, $scan_step
if ($Success_2d){
$modem_strength = getModemStrength();
$xml_state = simplexml_load_file($motosat_url);
$DishAngleSat = $xml_state->DishAngle;
$DishAngleSat = getDishAngles($xml_state); // ->DishAngle;
$satellite = array(
'az' => $DishAngleSat[0],
"el" => $DishAngleSat[1],
......@@ -496,8 +543,8 @@ function peak2D($xml_state, $az_play, $el_play, $az_range, $el_range, $az_step,
}
if ($dbg_file !== null) {
fprintf($dbg_file, "==== 2D scan strengths: ====\n");
fprintf($dbg_file, print_r($scan_strengths, 1));
fprintf($dbg_file, "==== 2D scan strengths (will be printed in findMax2d() ): ====\n");
// fprintf($dbg_file, print_r($scan_strengths, 1));
}
// $rel_argmax = findMax1d($scan_strengths, $fract, $pow, $argmax_outside);
......@@ -518,7 +565,7 @@ function peak2D($xml_state, $az_play, $el_play, $az_range, $el_range, $az_step,
}
}
if ($dbg_file !== null) {
fprintf($dbg_file, "reltive argmax = [az=%f, el=%f]\n", $rel_argmax[0], $rel_argmax[1]);
fprintf($dbg_file, "relative argmax = [az=%f, el=%f]\n", $rel_argmax[0], $rel_argmax[1]);
fprintf($dbg_file, " argmax = [az=%f, el=%f] (was [az=%f, el=%f])\n", $azel_argmax[0], $azel_argmax[1], $azel_start_center[0], $azel_start_center[1]);
}
$DishAngle[0] = $azel_argmax[0] - $az_play;
......@@ -560,10 +607,16 @@ function peakAzimuthOrElevation($xml_state, $el_not_az, $azel_play, $azel_range,
if ($azel_min < $minmax[0]){
$azel_min = $minmax[0];
}
if ($azel_max < $minmax[1]){
if ($azel_max > $minmax[1]){
$azel_max = $minmax[1];
}
}
if ($dbg_file !== null) {
fprintf($dbg_file, "Peaking %s\n",$direction);
}
if (($azel_max - $azel_min) < $azel_step){
if ($dbg_file !== null) {
fprintf($dbg_file, "==== $direction scan range to small min = %f, max = %f: ====\n", $azel_min, $azel_max);
......@@ -599,7 +652,7 @@ function peakAzimuthOrElevation($xml_state, $el_not_az, $azel_play, $azel_range,
$azel_argmax = $azel_min + $azel_step * $rel_argmax;
if ($dbg_file !== null) {
fprintf($dbg_file, "reltive argmax = %f\n", $rel_argmax);
fprintf($dbg_file, "relative argmax = %f\n", $rel_argmax);
fprintf($dbg_file, "$direction argmax = %f (was %f)\n", $azel_argmax, $azel_start_center);
}
$DishAngle[$motor] = $azel_argmax - $azel_play;
......@@ -800,11 +853,15 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){
$min= $data[0];
$max = $data[0];
$argmax = 0;
foreach ($data as $k => $v){
foreach ($data as $k => $v) {
if (! in_array($v, $HUGHES_BAD_STRENGTHS)) {
if ($v > $max) {
$max = $v;
$argmax = $k;
} else if ($v < $min) $min = $v;
} else if ($v < $min) {
$min = $v;
}
}
}
if ($argmax == 0){
......@@ -815,7 +872,7 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){
return array('argmax'=>$argmax,'err'=>'last element');
}
// ignoring bad strengths (==30) - they will be filtered later
$threshold = $min + ($max-$min) * $fract;
$k_min = $argmax;
while (($k_min > 0) && ($data[$k_min - 1] >= $threshold)){
......@@ -844,8 +901,6 @@ function findMax1d($data, $fract, $pow, $frac_outside = 0.2){
$pa = new PolynomialApproximation();
$pa->debugLevel = 1;
$pa->debugFile = $dbg_file;
$rslt = $pa->polynomialApproximation1d($poly_data, 2);
......@@ -947,17 +1002,17 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step,
fprintf($dbg_file, "threshold=%f:\n",$threshold);
fprintf($dbg_file, "scan_step=%f:\n",$scan_step);
fprintf($dbg_file, "DishAngle - start:\n");
fprintf($dbg_file, print_r($DishAngle,1));
fprintf($dbg_file, str_replace("\n"," ",print_r($DishAngle,1))."\n");
fprintf($dbg_file, "DishAngleCenter - start:\n");
fprintf($dbg_file, print_r($DishAngleCenter,1));
fprintf($dbg_file, str_replace("\n"," ",print_r($DishAngleCenter,1))."\n");
}
while (null !==moveElAzSk_degrees($DishAngle));
// while (null !==moveElAzSk_degrees($DishAngle)); // returns number of steps
moveElAzSk_degrees($DishAngle); // returns number of steps
$DishAngle[0] = $az_end;
if ($dbg_file) {
fprintf($dbg_file, "\nDishAngle - end:\n");
fprintf($dbg_file, print_r($DishAngle,1));
fprintf($dbg_file, str_replace("\n"," ",print_r($DishAngle,1))."\n");
}
moveElAzSk_start_degrees($DishAngle);
......@@ -969,8 +1024,8 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step,
$xml_state = simplexml_load_file($motosat_url);
if ($dbg_file) {
$DishAngle = getDishAngles ($xml_state);
fprintf($dbg_file, "\nDishAngle - after hitting/stopped:\n");
fprintf($dbg_file, print_r($DishAngle,1));
fprintf($dbg_file, "\n***** DishAngle - after hitting/stopped:\n");
fprintf($dbg_file, str_replace("\n"," ",print_r($DishAngle,1))."\n");
}
//,$LATE_STOP_AZ
......@@ -986,7 +1041,7 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step,
$xml_state = simplexml_load_file($motosat_url);
$DishAngle = getDishAngles ($xml_state);
fprintf($dbg_file, "\nDishAngle - after backing:\n");
fprintf($dbg_file, print_r($DishAngle,1));
fprintf($dbg_file, str_replace("\n"," ",print_r($DishAngle,1))."\n");
}
$xml_state->addChild ('Success',(string) 1 );
return $xml_state;
......@@ -995,8 +1050,8 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step,
$dir = 1 - $dir;
}
moveElAzSk_degrees($DishAngleCenter);
while (null !==moveElAzSk_degrees($DishAngleCenter));
// while (null !==moveElAzSk_degrees($DishAngleCenter));
moveElAzSk_degrees($DishAngleCenter);
$xml_state = simplexml_load_file($motosat_url);
$xml_state->addChild ('Success',(string) 0 );
return $xml_state;
......@@ -1067,8 +1122,10 @@ function moveElAzSk_start_counts($new_azelsk_count) {
}
$dist = abs($dist);
$url = $motosat_url."?Action=".$mode.$dir.'&Distance='.$dist."&Units=0";
if ($dbg_file) {
fprintf($dbg_file, "moveElAzSk_start_counts(): url = %s:\n",$url);
}
if (false && $dbg_file) {
fprintf($dbg_file, "url = %s:\n",$url);
fprintf($dbg_file, "new_azelsk_count:\n");
fprintf($dbg_file, print_r($new_azelsk_count,1));
fprintf($dbg_file, "DishCount:\n");
......@@ -1100,8 +1157,8 @@ function moveElAzSk_start_degrees($new_azelsk_degree) {
($new_azelsk_degree[0] + $AZ_0)*$AZ_CPD,
($new_azelsk_degree[1] + $EL_0)*$EL_CPD,
($new_azelsk_degree[2] + $SK_0)*$SK_CPD);
fprintf($dbg_file, "new_azelsk_degree:\n");
fprintf($dbg_file, print_r($new_azelsk_degree,1));
fprintf($dbg_file, ".... moveElAzSk_start_degrees(): new_azelsk_degree: ");
fprintf($dbg_file, str_replace("\n"," ",print_r($new_azelsk_degree,1))."\n");
// fprintf($dbg_file, "new_azelsk_count_dbg:\n");
// fprintf($dbg_file, print_r($new_azelsk_count_dbg,1));
// fprintf($dbg_file, "new_azelsk_count:\n");
......@@ -1192,17 +1249,27 @@ function moveElAzSk_counts($new_azelsk_count, $multi = true)
function moveElAzSk_degrees($new_azelsk_degree, $multi = true)
{
global $dbg_file;
$total_cycles = 0;
while (true) {
$xml_state = moveElAzSk_start_degrees($new_azelsk_degree);
if (isset($xml_state)) {
$cycles = waitMotorsReady();
$total_cycles += $cycles;
if (!$multi)
if (! $multi) {
if ($dbg_file) {
fprintf($dbg_file, "moveElAzSk_degrees(), multi=" . $multi . ", total_cycles=" . $total_cycles . "\n");
}
return $total_cycles;
} else
}
} else {
if ($dbg_file) {
fprintf($dbg_file, "moveElAzSk_degrees(), multi=" . $multi . ", total_cycles=" . $total_cycles . ", xml_state is NOT set\n");
// fprintf($dbg_file, print_r(debug_backtrace(),1));
}
return $total_cycles; // did not move last time
}
}
}
function myval ($s) {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment