Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
M
motosat
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Board
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Elphel
motosat
Commits
42caa731
Commit
42caa731
authored
Sep 08, 2019
by
Andrey Filippov
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
ported 1d polynomial approximation from Java code
parent
eca9882b
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
476 additions
and
186 deletions
+476
-186
PolynomialApproximation.php
PolynomialApproximation.php
+265
-0
test_01.php
test_01.php
+211
-186
No files found.
PolynomialApproximation.php
0 → 100644
View file @
42caa731
<?php
//require_once ('test-matrix-invert.php'); // matrix inversion
class
PolynomialApproximation
{
// function __construct() {
// }
public
$debugLevel
=
1
;
public
$debugFile
=
null
;
public
function
polynomialApproximation1d
(
$data
,
$N
){
//$my_array = array_fill(0, $size_of_the_array, $some_data);
if
(
$this
->
debugFile
===
null
){
$this
->
debugLevel
=
0
;
}
$S
=
array_fill
(
0
,
2
*
$N
+
1
,
0
);
// new double [2*N+1];
$SF
=
array_fill
(
0
,
$N
+
1
,
0
);
// new double [N+1];
for
(
$i
=
0
;
$i
<
sizeof
(
$data
);
$i
++
){
$wxn
=
(
sizeof
(
$data
[
$i
])
>
2
)
?
$data
[
$i
][
2
]
:
1.0
;
if
(
$wxn
>
0.0
){
// save time on 0.0 that can be used to mask out some samples
$f
=
$data
[
$i
][
1
];
$x
=
$data
[
$i
][
0
];
for
(
$j
=
0
;
$j
<=
$N
;
$j
++
){
$S
[
$j
]
+=
$wxn
;
$SF
[
$j
]
+=
$wxn
*
$f
;
$wxn
*=
$x
;
}
for
(
$j
=
$N
+
1
;
$j
<
2
*
$N
;
$j
++
){
$S
[
$j
]
+=
$wxn
;
$wxn
*=
$x
;
}
$S
[
2
*
$N
]
+=
$wxn
;
if
(
$this
->
debugLevel
>
1
){
if
(
sizeof
(
$data
[
$i
])
>
2
)
fprintf
(
$this
->
debugFile
,
"polynomialApproximation1d() |"
.
$i
.
"|: x=|"
.
$data
[
$i
][
0
]
.
"| f(x)=|"
.
$data
[
$i
][
1
]
.
"| (w=
\t
|"
.
$data
[
$i
][
2
]
.
"|
\t
)"
);
else
fprintf
(
$this
->
debugFile
,
"polynomialApproximation1d() |"
.
$i
,
"|: x=|"
.
$data
[
$i
][
0
]
.
"| f(x)=|"
.
$data
[
$i
][
1
]
.
"|
\t
)"
);
}
}
}
$M
=
$this
->
zeroMatrix
(
$N
+
1
,
$N
+
1
);
$B
=
$this
->
zeroMatrix
(
$N
+
1
,
1
);
for
(
$i
=
0
;
$i
<=
$N
;
$i
++
)
{
$B
[
$i
][
0
]
=
$SF
[
$i
];
for
(
$j
=
0
;
$j
<=
$N
;
$j
++
)
$M
[
$i
][
$j
]
=
$S
[
$i
+
$j
];
}
$N1
=
$N
;
// TODO: use try/catch with solve
if
(
$this
->
debugLevel
>
1
){
fprintf
(
$this
->
debugFile
,
"polynomialApproximation1d(data,"
.
$N
.
") M:
\n
"
);
$this
->
print_matrix
(
$this
->
debugFile
,
$M
);
fprintf
(
$this
->
debugFile
,
"polynomialApproximation1d() B:
\n
"
);
$this
->
print_matrix
(
$this
->
debugFile
,
$B
);
}
// while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){
$df
=
(
$this
->
debugLevel
>
2
)
?
$this
->
debugFile
:
null
;
while
(
$N1
>=
0
)
{
// make N=0 legal ?
try
{
$M_inv
=
$this
->
matrixInvert
(
$M
,
$df
);
$R
=
$this
->
mmul
(
$M_inv
,
$B
);
if
(
$this
->
debugLevel
>
1
){
fprintf
(
$this
->
debugFile
,
"polynomialApproximation1d() solution=
\n
"
);
$this
->
print_matrix
(
$this
->
debugFile
,
$R
);
}
$result
=
array_fill
(
0
,
$N
+
1
,
0
);
// new double [N+1];
for
(
$i
=
0
;
$i
<=
$N1
;
$i
++
){
$result
[
$i
]
=
$R
[
$i
][
0
];
}
return
$result
;
}
catch
(
Exception
$e
)
{
echo
'Caught exception: '
,
$e
->
getMessage
(),
", reducing polynomial order
\n
"
;
if
(
$N1
<
0
)
{
return
null
;
}
$N1
--
;
$M1
=
$this
->
zeroMatrix
(
$N
+
1
,
$N
+
1
);
$B1
=
$this
->
zeroMatrix
(
$N
+
1
,
1
);
for
(
$i
=
0
;
$i
<=
$N1
;
$i
++
)
{
$B1
[
$i
][
0
]
=
$B
[
$i
][
0
];
for
(
$j
=
0
;
$j
<=
$N1
;
$j
++
){
$M1
[
$i
][
$j
]
=
$M
[
$i
][
$j
];
}
}
$M
=
$M1
;
$B
=
$B1
;
}
}
return
null
;
}
public
function
mmul
(
$A
,
$B
){
$M
=
sizeof
(
$A
);
$N
=
sizeof
(
$A
[
0
]);
if
(
$N
!=
sizeof
(
$B
)){
throw
new
Exception
(
'DImensions mismatch.'
);
}
$K
=
sizeof
(
$B
[
0
]);
$R
=
$this
->
zeroMatrix
(
$M
,
$K
);
for
(
$i
=
0
;
$i
<
$M
;
$i
++
){
for
(
$j
=
0
;
$j
<
$K
;
$j
++
){
for
(
$k
=
0
;
$k
<
$N
;
$k
++
){
$R
[
$i
][
$j
]
+=
$A
[
$i
][
$k
]
*
$B
[
$k
][
$j
];
}
}
}
return
$R
;
}
public
function
zeroMatrix
(
$m
,
$n
=
1
){
$M
=
array_fill
(
0
,
$m
,
null
);
for
(
$i
=
0
;
$i
<
$m
;
$i
++
){
$M
[
$i
]
=
array_fill
(
0
,
$n
,
0.0
);
}
return
$M
;
}
/**
* Inverts a given matrix(downloaded from https://gist.github.com/unix1/7510208 )
*
* @param array $A matrix to invert
* @param string $debugFile if not null - output file for debug info
*
* @return array inverted matrix
*/
public
function
matrixInvert
(
$A
,
$debugFile
=
null
)
{
/// @todo check rows = columns
$n
=
count
(
$A
);
// get and append identity matrix
$I
=
$this
->
identity_matrix
(
$n
);
for
(
$i
=
0
;
$i
<
$n
;
++
$i
)
{
$A
[
$i
]
=
array_merge
(
$A
[
$i
],
$I
[
$i
]);
}
if
(
$debugFile
!==
null
)
{
fprintf
(
$debugFile
,
"
\n
Starting matrix:
\n
"
);
// echo "\nStarting matrix: ";
$this
->
print_matrix
(
$debugFile
,
$A
);
}
// forward run
for
(
$j
=
0
;
$j
<
$n
-
1
;
++
$j
)
{
// for all remaining rows (diagonally)
for
(
$i
=
$j
+
1
;
$i
<
$n
;
++
$i
)
{
// if the value is not already 0
if
(
$A
[
$i
][
$j
]
!==
0
)
{
// adjust scale to pivot row
// subtract pivot row from current
$scalar
=
$A
[
$j
][
$j
]
/
$A
[
$i
][
$j
];
for
(
$jj
=
$j
;
$jj
<
$n
*
2
;
++
$jj
)
{
$A
[
$i
][
$jj
]
*=
$scalar
;
$A
[
$i
][
$jj
]
-=
$A
[
$j
][
$jj
];
}
}
}
// if ($debug) {
if
(
$debugFile
!==
null
)
{
fprintf
(
$debugFile
,
"
\n
Forward iteration
$j
:
\n
"
);
// echo "\nForward iteration $j: ";
$this
->
print_matrix
(
$debugFile
,
$A
);
}
}
// reverse run
for
(
$j
=
$n
-
1
;
$j
>
0
;
--
$j
)
{
for
(
$i
=
$j
-
1
;
$i
>=
0
;
--
$i
)
{
if
(
$A
[
$i
][
$j
]
!==
0
)
{
$scalar
=
$A
[
$j
][
$j
]
/
$A
[
$i
][
$j
];
for
(
$jj
=
$i
;
$jj
<
$n
*
2
;
++
$jj
)
{
$A
[
$i
][
$jj
]
*=
$scalar
;
$A
[
$i
][
$jj
]
-=
$A
[
$j
][
$jj
];
}
}
}
// if ($debug) {
if
(
$debugFile
!==
null
)
{
fprintf
(
$debugFile
,
"
\n
Reverse iteration
$j
:
\n
"
);
// echo "\nReverse iteration $j: ";
$this
->
print_matrix
(
$debugFile
,
$A
);
}
}
// last run to make all diagonal 1s
/// @note this can be done in last iteration (i.e. reverse run) too!
for
(
$j
=
0
;
$j
<
$n
;
++
$j
)
{
if
(
$A
[
$j
][
$j
]
!==
1
)
{
if
(
$A
[
$j
][
$j
]
==
0.0
)
{
// echo "Denominator is zero, throwing exception: \n";
throw
new
Exception
(
'Singular matrix.'
);
}
$scalar
=
1
/
$A
[
$j
][
$j
];
for
(
$jj
=
$j
;
$jj
<
$n
*
2
;
++
$jj
)
{
$A
[
$j
][
$jj
]
*=
$scalar
;
}
}
// if ($debug) {
if
(
$debugFile
!==
null
)
{
fprintf
(
$debugFile
,
"
\n
1-out iteration
$j
:
\n
"
);
// echo "\n1-out iteration $j: ";
$this
->
print_matrix
(
$debugFile
,
$A
);
}
}
// take out the matrix inverse to return
$Inv
=
array
();
for
(
$i
=
0
;
$i
<
$n
;
++
$i
)
{
$Inv
[
$i
]
=
array_slice
(
$A
[
$i
],
$n
);
}
return
$Inv
;
}
/**
* Prints matrix
* @param resource $debugFile debug file or null
* @param array $A matrix
* @param integer $decimals number of decimals
*/
function
print_matrix
(
$debugFile
,
$A
,
$decimals
=
6
)
{
if
(
$debugFile
===
null
){
$debugFile
=
fopen
(
'php://stdout'
,
'w'
);
}
foreach
(
$A
as
$row
)
{
fprintf
(
$debugFile
,
"["
);
foreach
(
$row
as
$i
)
{
fprintf
(
$debugFile
,
"
\t
"
.
sprintf
(
"%01.
{
$decimals
}
f"
,
round
(
$i
,
$decimals
)));
}
fprintf
(
$debugFile
,
"
\t
]
\n
"
);
}
}
/**
* Produces an identity matrix of given size
*
* @param integer $n size of identity matrix
*
* @return array identity matrix
*/
function
identity_matrix
(
$n
)
{
$I
=
array
();
for
(
$i
=
0
;
$i
<
$n
;
++
$i
)
{
for
(
$j
=
0
;
$j
<
$n
;
++
$j
)
{
$I
[
$i
][
$j
]
=
(
$i
==
$j
)
?
1
:
0
;
}
}
return
$I
;
}
}
test_01.php
View file @
42caa731
<?php
<?php
require_once
(
'PolynomialApproximation.php'
);
$motosat_ip
=
"192.168.1.250"
;
$motosat_ip
=
"192.168.1.250"
;
$motosat_url
=
"http://"
.
$motosat_ip
.
"/motor.xml"
;
$motosat_url
=
"http://"
.
$motosat_ip
.
"/motor.xml"
;
$modem_ip
=
"192.168.0.1"
;
$modem_ip
=
"192.168.0.1"
;
//$modem_ip = "http://localhost/motosat/attic";
//$modem_ip = "http://localhost/motosat/attic";
//$modem_strengt_url = $modem_ip."/sqf/sqfdisp/index.html";
//$modem_strengt_url = $modem_ip."/sqf/sqfdisp/index.html";
//$modem_strengt_url_good = "http://localhost/motosat/attic/sqf/sqfdisp/index.html";
//$modem_strengt_url_bad = "http://localhost/motosat/attic/sqf/sqfdisp/index.html1";
//$modem_strengt_url = "http://192.168.0.1/sqf/sqfdisp/index.html";
$modem_strengt_url
=
"http://"
.
$modem_ip
.
"/sqf/sqfdisp/index.html"
;
$modem_strengt_url
=
"http://"
.
$modem_ip
.
"/sqf/sqfdisp/index.html"
;
$AZ_CPD
=
12.74445
;
$AZ_CPD
=
12.74445
;
$EL_CPD
=
75.78567
;
$EL_CPD
=
75.78567
;
...
@@ -17,6 +21,14 @@ $SK_CMAX = 931;
...
@@ -17,6 +21,14 @@ $SK_CMAX = 931;
$MODEM_THRESHOLD
=
17.0
;
// consider something if modem strength is above
$MODEM_THRESHOLD
=
17.0
;
// consider something if modem strength is above
$OVERSHOOT
=
1
;
// final steps absolute error
$OVERSHOOT
=
1
;
// final steps absolute error
$LATE_STOP_AZ
=
30
;
// steps
$LATE_STOP_AZ
=
30
;
// steps
// Rescan azimuth after noticed a satellite during area scan
$AZ_PLAY
=
1.0
;
// Hysteresis caused by mechanical play (degrees)
$AZ_RESCAN
=
8
;
// (degrees) around expected center
$AZ_RESCAN_STEP
=
0.25
;
// (degrees)
$USABLE_FRACT
=
0.6
;
// use ony samples not less than this fraction of maximal
$USABLE_POW
=
1.0
;
// transform (maxstrengh - strengh) by applying pow - 1.2 ...1.3 may have marginal improvements
$ARGMAX_OURSIDE
=
0.2
;
// Allow argmax outside of the measured range by this fraction
/*
/*
http://192.168.1.250/motor.xml?Action=ElUp&Distance=0.5&Units=1 0.5 degree up
http://192.168.1.250/motor.xml?Action=ElUp&Distance=0.5&Units=1 0.5 degree up
...
@@ -30,155 +42,21 @@ http://192.168.1.250/motor.xml?Action=SkUp&Distance=1&Units=0 1 step sk +
...
@@ -30,155 +42,21 @@ http://192.168.1.250/motor.xml?Action=SkUp&Distance=1&Units=0 1 step sk +
http://192.168.1.250/motor.xml?Action=SkDn&Distance=1&Units=0 1 step sk -
http://192.168.1.250/motor.xml?Action=SkDn&Distance=1&Units=0 1 step sk -
http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop
http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop
$xml_state = simplexml_load_file("http://192.168.1.250/motor.xml");
LEDState -> 1344
SignalQuality -> 15.00, 0.00, 99.00
SignalStrength -> 31.18
SysStatus -> System initialized
SkMotor -> 1
SatSys -> 1
MotorState -> 0
DishAngle -> 9.97, 0.00, 0.00
DishCount -> 127, 0, 465
php > var_dump ($xml_state);
object(SimpleXMLElement)#1 (10) {
["State"]=> string(1) "1"
["LEDState"]=> string(4) "1344"
["SignalQuality"]=> string(18) "15.00, 0.00, 99.00"
["SignalStrength"]=> string(5) "31.18"
["SysStatus"]=> string(18) "System initialized"
["SkMotor"]=> string(1) "1"
["SatSys"]=> string(1) "1"
["MotorState"]=> string(1) "0"
["DishAngle"]=> string(16) "9.97, 0.00, 0.00"
["DishCount"]=> string(11) "127, 0, 465"
}
["DishAngle"]=> string(22) "196.48, 114.98, -12.95"
["DishCount"]=> string(15) "2504, 8714, 357" round to the
*/
// Read Motosat state:
/*
$xml_state = simplexml_load_file($motosat_url);
$SignalQuality = (explode(",",$xml_state->SignalQuality)[0]+0.0);
$State = $xml_state->State+0; // 1 - Ready, 2 - Busy
$DishAngle = explode(",",$xml_state->DishAngle);
foreach ($DishAngle as $key => $value){
$DishAngle[$key] = $value + 0.0;
}
$DishCount = explode(",",$xml_state->DishCount);
foreach ($DishCount as $key => $value){
$DishCount[$key] = $value + 0;
}
localhost/motosat/test_01.php?el=114.98&az=196.48&nodbg
8714: 114.98
<!--array(3) {
[0]=>
float(196.48)
[1]=>
float(114)
[2]=>
float(-12.95)
}
array(3) {
[0]=>
float(2504.029536)
[1]=>
float(8639.56638)
[2]=>
float(356.997036)
}
array(3) {
[0]=>
int(2504)
[1]=>
int(8640)
[2]=>
int(357)
}
--><!--array(3) {
[0]=>
int(2504)
[1]=>
int(8640)
[2]=>
int(357)
}
array(3) {
[0]=>
int(2504)
[1]=>
int(8714)
[2]=>
int(357)
}
string(62) "http://192.168.1.250/motor.xml?Action=ElDn&Distance=74&Units=0"
--><?xml version="1.0"?>
<motosat><cycles>1</cycles></motosat>
8639: 113.99
view-source:http://localhost/motosat/test_01.php?el=114.98
<!--array(3) {
[0]=>
float(196.48)
[1]=>
float(114.98)
[2]=>
float(-12.95)
}
array(3) {
[0]=>
float(2504.029536)
[1]=>
float(8713.8363366)
[2]=>
float(356.997036)
}
array(3) {
[0]=>
int(2504)
[1]=>
int(8714)
[2]=>
int(357)
}
--><!--array(3) {
[0]=>
int(2504)
[1]=>
int(8714)
[2]=>
int(357)
}
array(3) {
[0]=>
int(2504)
[1]=>
int(8639)
[2]=>
int(357)
}
string(62) "http://192.168.1.250/motor.xml?Action=ElUp&Distance=75&Units=0"
--><?xml version="1.0"?>
<motosat><cycles>1</cycles></motosat>
string(61) "http://192.168.1.250/motor.xml?Action=ElDn&Distance=1&Units=0"
--><?xml version="1.0"?>
<motosat><cycles>1</cycles></motosat>
*/
*/
//php > var_dump($DishAngle);
//php > var_dump($DishAngle);
//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}
//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}
$dbg_file
=
fopen
(
"/home/eyesis/git/motosat/attic/logs/
modem
.log"
,
"w"
);
$dbg_file
=
fopen
(
"/home/eyesis/git/motosat/attic/logs/
test03
.log"
,
"w"
);
fprintf
(
$dbg_file
,
"test log
\n
"
);
fprintf
(
$dbg_file
,
"test log
\n
"
);
if
(
false
)
{
$data
=
Array
(
5
,
6
,
7
,
9
,
9
,
10
,
11
,
11
,
12
,
12
,
11
,
11
,
10
,
10
,
10
,
36
,
40
,
45
,
47
,
51
,
51
,
51
,
49
,
48
,
43
,
38
,
30
,
30
,
30
,
10
,
10
,
10
,
10
);
$rslt
=
findMax1d
(
$data
,
$USABLE_FRACT
,
$USABLE_POW
,
$ARGMAX_OURSIDE
);
print_r
(
$rslt
);
print
(
"
\n
"
);
exit
(
0
);
}
if
(
false
)
{
if
(
false
)
{
print
(
"<p>"
.
date
(
'l jS \of F Y h:i:s A'
)
.
"</p>"
);
print
(
"<p>"
.
date
(
'l jS \of F Y h:i:s A'
)
.
"</p>"
);
$modem_strength
=
getModemStrength
();
$modem_strength
=
getModemStrength
();
...
@@ -192,15 +70,9 @@ if (false) {
...
@@ -192,15 +70,9 @@ if (false) {
}
}
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
// use current as default gor HTTP GET
$DishCount
=
explode
(
","
,
$xml_state
->
DishCount
);
//$DishCount = getDishCounts (xml_state);
foreach
(
$DishCount
as
$key
=>
$value
){
$DishAngle
=
getDishAngles
(
$xml_state
);
$DishCount
[
$key
]
=
$value
+
0
;
}
$DishAngle
=
explode
(
","
,
$xml_state
->
DishAngle
);
foreach
(
$DishAngle
as
$key
=>
$value
){
$DishAngle
[
$key
]
=
floatval
(
$value
);
}
$dbg
=
false
;
// true;
$dbg
=
false
;
// true;
$multi
=
true
;
// false;
$multi
=
true
;
// false;
...
@@ -223,18 +95,22 @@ $scan_mode = ($wnd_az > 0) || ($wnd_el > 0);
...
@@ -223,18 +95,22 @@ $scan_mode = ($wnd_az > 0) || ($wnd_el > 0);
if
(
$scan_mode
){
if
(
$scan_mode
){
$xml_state
=
scan_window
(
$min_strength
,
$DishAngle
,
$wnd_az
,
$wnd_el
,
$scan_step
);
$xml_state
=
scan_window
(
$min_strength
,
$DishAngle
,
$wnd_az
,
$wnd_el
,
$scan_step
);
$Success
=
$xml_state
->
Success
;
if
(
intval
(
$Success
)){
peakAzimuth
(
$xml_state
,
$AZ_PLAY
,
$AZ_RESCAN
,
$AZ_RESCAN_STEP
,
$USABLE_FRACT
,
$USABLE_POW
,
$ARGMAX_OURSIDE
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
}
$xml
=
new
SimpleXMLElement
(
"<?xml version='1.0'?><motosat/>"
);
$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
$busy
=
(
$State
&
2
)
!=
0
;
$busy
=
(
$State
&
2
)
!=
0
;
$SignalQuality
=
myfval
(
explode
(
","
,
$xml_state
->
SignalQuality
)[
0
]);
$SignalQuality
=
myfval
(
explode
(
","
,
$xml_state
->
SignalQuality
)[
0
]);
$DishCount
=
$xml_state
->
DishCount
;
$DishAngle
=
$xml_state
->
DishAngle
;
$Success
=
$xml_state
->
Success
;
$xml
->
addChild
(
'Success'
,(
string
)
$Success
);
$xml
->
addChild
(
'Success'
,(
string
)
$Success
);
$xml
->
addChild
(
'SignalQuality'
,(
string
)
$SignalQuality
);
$xml
->
addChild
(
'SignalQuality'
,(
string
)
$SignalQuality
);
$xml
->
addChild
(
'DishAngle'
,
(
string
)
$
DishAngle
);
$xml
->
addChild
(
'DishAngle'
,
$xml_state
->
DishAngle
);
$xml
->
addChild
(
'DishCount'
,
(
string
)
$DishCoun
t
);
$xml
->
addChild
(
'DishCount'
,
$xml_state
->
DishCount
t
);
$xml
->
addChild
(
'busy'
,(
string
)
$busy
);
$xml
->
addChild
(
'busy'
,(
string
)
$busy
);
$xml
->
addChild
(
'State'
,(
string
)
$State
);
$xml
->
addChild
(
'State'
,(
string
)
$State
);
...
@@ -271,6 +147,141 @@ header('Access-Control-Allow-Origin: *');
...
@@ -271,6 +147,141 @@ header('Access-Control-Allow-Origin: *');
printf
(
$rslt
);
printf
(
$rslt
);
exit
(
0
);
exit
(
0
);
function
peakAzimuth
(
$xml_state
,
$az_play
,
$az_range
,
$az_step
,
$fract
,
$pow
=
1.0
,
$argmax_outside
=
0.2
){
global
$motosat_url
,
$dbg_file
;
if
(
$xml_state
===
null
){
$xml_state
=
simplexml_load_file
(
$motosat_url
);
}
$DishAngle
=
getDishAngles
(
$xml_state
);
$az_start_center
=
$DishAngle
[
0
];
//move to sart rescan azimuth minus mechanical play
$DishAngle
[
0
]
=
$az_start_center
-
$az_play
-
$az_range
/
2
;
moveElAzSk_degrees
(
$DishAngle
);
$scan_strengths
=
Array
();
for
(
$az
=
$az_start_center
-
$az_range
/
2
;
$az
<
(
$az_start_center
+
$az_range
/
2
+
$az_step
);
$az
+=
$az_step
){
$DishAngle
[
0
]
=
$az
;
moveElAzSk_degrees
(
$DishAngle
);
// to be on the safe side re-read xml
$modem_strength
=
getModemStrength
();
$scan_strengths
[]
=
$modem_strength
;
}
if
(
$dbg_file
!==
null
)
{
fprintf
(
$dbg_file
,
"==== Azimuth scan strengths: ====
\n
"
);
fprintf
(
$dbg_file
,
print_r
(
$scan_strengths
,
1
));
}
$rel_argmax
=
findMax1d
(
$scan_strengths
,
$fract
,
$pow
,
$argmax_outside
);
if
(
$rel_argmax
!==
null
)
{
$az_argmax
=
$az_start_center
-
$az_range
/
2
+
$az_step
*
$rel_argmax
;
if
(
$dbg_file
!==
null
)
{
fprintf
(
$dbg_file
,
"reltive argmax = %f
\n
"
,
$rel_argmax
);
fprintf
(
$dbg_file
,
"azimuth argmax = %f (was %f)
\n
"
,
$az_argmax
,
$az_start_center
);
}
$DishAngle
[
0
]
=
$az_argmax
-
$az_play
;
moveElAzSk_degrees
(
$DishAngle
);
$DishAngle
[
0
]
=
$az_argmax
;
moveElAzSk_degrees
(
$DishAngle
);
}
else
{
// for now - move back
if
(
$dbg_file
!==
null
)
{
fprintf
(
$dbg_file
,
"****** Failed to find maximum, going to known position.
\n
"
);
}
$DishAngle
[
0
]
=
$az_start_center
;
moveElAzSk_degrees
(
$DishAngle
);
}
return
;
}
function
findMax1d
(
$data
,
$fract
,
$pow
,
$frac_outside
=
0.2
){
global
$dbg_file
;
$min
=
$data
[
0
];
$max
=
$data
[
0
];
$argmax
=
0
;
foreach
(
$data
as
$k
=>
$v
){
if
(
$v
>
$max
)
{
$max
=
$v
;
$argmax
=
$k
;
}
else
if
(
$v
<
$min
)
$min
=
$v
;
}
$threshold
=
$min
+
(
$max
-
$min
)
*
$fract
;
$k_min
=
$argmax
;
while
((
$k_min
>
0
)
&&
(
$data
[
$k_min
-
1
]
>=
$threshold
)){
$k_min
--
;
}
$k_max
=
$argmax
;
$last
=
sizeof
(
$data
)
-
1
;
while
((
$k_max
<
$last
)
&&
(
$data
[
$k_max
+
1
]
>=
$threshold
)){
$k_max
++
;
}
$datap
=
array_fill
(
0
,
sizeof
(
$data
),
0
);
for
(
$k
=
$k_min
;
$k
<=
$k_max
;
$k
++
){
$datap
[
$k
]
=
$max
-
pow
(
$max
-
$data
[
$k
],
$pow
);
}
$poly_data
=
Array
();
for
(
$k
=
$k_min
;
$k
<=
$k_max
;
$k
++
){
$poly_data
[]
=
Array
(
$k
-
$argmax
,
$datap
[
$k
]);
}
$pa
=
new
PolynomialApproximation
();
$rslt
=
$pa
->
polynomialApproximation1d
(
$poly_data
,
2
);
if
(
$dbg_file
)
{
fprintf
(
$dbg_file
,
"argmax= %d , k_min= %d, k_max = %d, max = %f, threshold = %f, rslt:
\n
"
,
$argmax
,
$k_min
,
$k_max
,
$max
,
$threshold
);
fprintf
(
$dbg_file
,
print_r
(
$rslt
,
1
));
}
if
(
$rslt
[
2
]
>=
0
){
if
(
$dbg_file
)
{
fprintf
(
$dbg_file
,
"Could not find maximum coeff =
\n
"
.
printr
(
$rslt
,
1
)
.
"
\n
"
);
}
return
null
;
// no maximum
}
/*
// debugging
$rslt[] = $argmax;
$rslt[] = $argmax - $rslt[1] / (2 * $rslt[2]);
$pre = array_fill(0, sizeof($data),0);
$diff= array_fill(0, sizeof($data),0);
for ($k = $k_min; $k <= $k_max; $k++){
$x = $k-$argmax;
$pre[$k]=$rslt[0]+$rslt[1]*$x+$rslt[2]*$x*$x;
$diff[$k]=$pre[$k]-$datap[$k];
}
for ($k = $k_min; $k <= $k_max; $k++){
printf("[%02d]: %8.3f %8.3f %8.3f %8.3f\n", $k, $data[$k], $datap[$k], $pre[$k], $diff[$k]);
}
print("data\n");
print_r($data);
print("datap\n");
print_r($datap);
print("---datap\n");
// print_r($pre);
// print_r($diff);
*/
// $frac_outside = 0.2;
$rel_argmax
=
$argmax
-
$rslt
[
1
]
/
(
2
*
$rslt
[
2
]);
if
(
$rel_argmax
<
-
(
$frac_outside
*
sizeof
(
$data
))){
if
(
$dbg_file
)
{
fprintf
(
$dbg_file
,
"argmax is too low = %f, range=%d
\n
"
,
$rel_argmax
,
sizeof
(
$data
)
);
}
return
null
;
// too low
}
if
(
$rel_argmax
>
((
1.0
+
$frac_outside
)
*
sizeof
(
$data
))){
if
(
$dbg_file
)
{
fprintf
(
$dbg_file
,
"argmax is too high = %f, range=%d
\n
"
,
$rel_argmax
,
sizeof
(
$data
)
);
}
return
null
;
// too low
}
return
$rel_argmax
;
// $rslt;
}
/**
/**
* Scan rectangular AZ/EL window arownd expected satellite position, alternating
* Scan rectangular AZ/EL window arownd expected satellite position, alternating
...
@@ -333,20 +344,14 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
...
@@ -333,20 +344,14 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
if
(
$scan_rslt
<
0
)
{
if
(
$scan_rslt
<
0
)
{
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
if
(
$dbg_file
)
{
if
(
$dbg_file
)
{
$DishAngle
=
explode
(
","
,
$xml_state
->
DishAngle
);
$DishAngle
=
getDishAngles
(
$xml_state
);
foreach
(
$DishAngle
as
$key
=>
$value
){
$DishAngle
[
$key
]
=
floatval
(
$value
);
}
fprintf
(
$dbg_file
,
"
\n
DishAngle - after hitting/stopped:
\n
"
);
fprintf
(
$dbg_file
,
"
\n
DishAngle - after hitting/stopped:
\n
"
);
fprintf
(
$dbg_file
,
print_r
(
$DishAngle
,
1
));
fprintf
(
$dbg_file
,
print_r
(
$DishAngle
,
1
));
}
}
//,$LATE_STOP_AZ
//,$LATE_STOP_AZ
//move az back to compensate for late stop
//move az back to compensate for late stop
$DishCount
=
explode
(
","
,
$xml_state
->
DishCount
);
$DishCount
=
getDishCounts
(
$xml_state
);
foreach
(
$DishCount
as
$key
=>
$value
){
$DishCount
[
$key
]
=
$value
+
0
;
}
if
(
$dir
>
0
){
if
(
$dir
>
0
){
$DishCount
[
0
]
+=
$LATE_STOP_AZ
;
$DishCount
[
0
]
+=
$LATE_STOP_AZ
;
}
else
{
}
else
{
...
@@ -355,17 +360,11 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
...
@@ -355,17 +360,11 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
moveElAzSk_counts
(
$DishCount
);
moveElAzSk_counts
(
$DishCount
);
if
(
$dbg_file
)
{
if
(
$dbg_file
)
{
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$DishAngle
=
explode
(
","
,
$xml_state
->
DishAngle
);
$DishAngle
=
getDishAngles
(
$xml_state
);
foreach
(
$DishAngle
as
$key
=>
$value
){
$DishAngle
[
$key
]
=
floatval
(
$value
);
}
fprintf
(
$dbg_file
,
"
\n
DishAngle - after backing:
\n
"
);
fprintf
(
$dbg_file
,
"
\n
DishAngle - after backing:
\n
"
);
fprintf
(
$dbg_file
,
print_r
(
$DishAngle
,
1
));
fprintf
(
$dbg_file
,
print_r
(
$DishAngle
,
1
));
}
}
$xml_state
->
addChild
(
'Success'
,(
string
)
1
);
$xml_state
->
addChild
(
'Success'
,(
string
)
true
);
return
$xml_state
;
return
$xml_state
;
}
}
}
}
...
@@ -375,25 +374,26 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
...
@@ -375,25 +374,26 @@ function scan_window($threshold, $DishAngleCenter, $wnd_az, $wnd_el, $scan_step)
while
(
null
!==
moveElAzSk_degrees
(
$DishAngleCenter
));
while
(
null
!==
moveElAzSk_degrees
(
$DishAngleCenter
));
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
->
addChild
(
'Success'
,(
string
)
false
);
$xml_state
->
addChild
(
'Success'
,(
string
)
0
);
return
$xml_state
;
return
$xml_state
;
}
}
//<b>49</b>
//<b><font color=red>9</font></b>
function
getModemStrength
(){
//0.2sec
function
getModemStrength
(){
//0.2sec
global
$modem_strengt_url
,
$dbg_file
;
global
$modem_strengt_url
,
$dbg_file
;
$modem_page
=
file_get_contents
(
"
$modem_strengt_url
"
);
$modem_page
=
file_get_contents
(
$modem_strengt_url
);
// $pstr="@<b>([^<]*)</b>@i";
$pstr
=
"@<b>(<font[^>]*>)?([^<]*)</@i"
;
$matches
=
null
;
// just define
$matches
=
null
;
// just define
preg_match
(
'@<b>([^<]*)</b>@i'
,
$modem_page
,
$matches
);
preg_match
(
$pstr
,
$modem_page
,
$matches
);
return
myfval
(
$matches
[
1
]);
// return myfval($matches[1]);
return
myfval
(
$matches
[
2
]);
}
}
function
moveElAzSk_start_counts
(
$new_azelsk_count
)
{
function
moveElAzSk_start_counts
(
$new_azelsk_count
)
{
global
$motosat_url
,
$AZ_CMAX
,
$EL_CMAX
,
$SK_CMAX
,
$OVERSHOOT
,
$dbg
,
$dbg_file
;
global
$motosat_url
,
$AZ_CMAX
,
$EL_CMAX
,
$SK_CMAX
,
$OVERSHOOT
,
$dbg
,
$dbg_file
;
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$xml_state
=
simplexml_load_file
(
$motosat_url
);
$DishCount
=
explode
(
","
,
$xml_state
->
DishCount
);
$DishCount
=
getDishCounts
(
$xml_state
);
foreach
(
$DishCount
as
$key
=>
$value
){
$DishCount
[
$key
]
=
intval
(
$value
);
}
if
(
$DishCount
[
0
]
<
0
)
$DishCount
[
0
]
=
0
;
if
(
$DishCount
[
0
]
<
0
)
$DishCount
[
0
]
=
0
;
if
(
$DishCount
[
1
]
<
0
)
$DishCount
[
1
]
=
0
;
if
(
$DishCount
[
1
]
<
0
)
$DishCount
[
1
]
=
0
;
if
(
$DishCount
[
2
]
<
0
)
$DishCount
[
2
]
=
0
;
if
(
$DishCount
[
2
]
<
0
)
$DishCount
[
2
]
=
0
;
...
@@ -531,10 +531,7 @@ function waitMotorsReady($modem_threshold = 0.0){ //
...
@@ -531,10 +531,7 @@ function waitMotorsReady($modem_threshold = 0.0){ //
}
}
// Now move back to where good signal strength was detected
// Now move back to where good signal strength was detected
$DishCountGood
=
explode
(
","
,
$xml_state
->
DishCount
);
$DishCountGood
=
getDishCounts
(
$xml_state
);
foreach
(
$DishCountGood
as
$key
=>
$value
){
$DishCountGood
[
$key
]
=
$value
+
0
;
}
moveElAzSk_counts
(
$DishCountGood
);
moveElAzSk_counts
(
$DishCountGood
);
if
(
$dbg_file
)
{
if
(
$dbg_file
)
{
...
@@ -582,6 +579,34 @@ function myfval ($s) {
...
@@ -582,6 +579,34 @@ function myfval ($s) {
$s
=
trim
(
$s
,
"
\"
"
);
$s
=
trim
(
$s
,
"
\"
"
);
return
floatval
(
$s
);
return
floatval
(
$s
);
}
}
/**
* Get 3 dish angles (az/el/sk) in degrees from the XML data received from controller
* @param $xml_state : XML data from the controller
* @return: array of 3 angles (degrees): azimuth, elevation, skew
*/
function
getDishAngles
(
$xml_state
){
$DishAngles
=
explode
(
","
,
$xml_state
->
DishAngle
);
foreach
(
$DishAngles
as
$key
=>
$value
){
$DishAngles
[
$key
]
=
floatval
(
$value
);
}
return
$DishAngles
;
}
/**
* Get 3 dish angles (az/el/sk) in counts from the XML data received from controller
* @param $xml_state : XML data from the controller
* @return: array of 3 angles (counts): azimuth, elevation, skew
*/
function
getDishCounts
(
$xml_state
){
$DishCounts
=
explode
(
","
,
$xml_state
->
DishCount
);
foreach
(
$DishCounts
as
$key
=>
$value
){
$DishCounts
[
$key
]
=
floatval
(
$value
);
}
return
$DishCounts
;
}
?>
?>
\ No newline at end of file
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment