Commit 14632e65 authored by Andrey Filippov's avatar Andrey Filippov

debugging exif, moved here other gps/imu related scripts

parent 09f51911
#TODO: pass $(LN) from bitbake
WWW_PAGES = /www/pages LN = ln
WWW_PAGES = /www/pages/
BINDIR = /usr/bin/ BINDIR = /usr/bin/
VARHTML = /var/volatile/html/
CONFDIR = /etc/elphel393/
INSTMODE = 0755 INSTMODE = 0755
INSTDOCS = 0644 INSTDOCS = 0644
INSTOWNER = root INSTOWNER = root
...@@ -9,7 +12,8 @@ INSTGROUP = root ...@@ -9,7 +12,8 @@ INSTGROUP = root
PROGS = nmea2exif log_imu PROGS = nmea2exif log_imu
SRCS = nmea2exif.c log_imu.c SRCS = nmea2exif.c log_imu.c
OBJS = nmea2exif.o OBJS = nmea2exif.o
PHPSCRIPTS= logger_launcher.php imu_setup.php start_gps_compass.php PHPSCRIPTS= start_gps_compass.php
PHPWEB= logger_launcher.php imu_setup.php
CFLAGS += -Wall -I$(STAGING_DIR_HOST)/usr/include-uapi CFLAGS += -Wall -I$(STAGING_DIR_HOST)/usr/include-uapi
LDLIBS += -lm LDLIBS += -lm
...@@ -27,7 +31,13 @@ install: $(PROGS) ...@@ -27,7 +31,13 @@ install: $(PROGS)
$(INSTALL) -d $(DESTDIR)$(BINDIR) $(INSTALL) -d $(DESTDIR)$(BINDIR)
$(INSTALL) -d $(DESTDIR)$(WWW_PAGES) $(INSTALL) -d $(DESTDIR)$(WWW_PAGES)
$(INSTALL) -m $(INSTMODE) -o $(INSTOWNER) -g $(INSTGROUP) $(PROGS) $(DESTDIR)$(BINDIR) $(INSTALL) -m $(INSTMODE) -o $(INSTOWNER) -g $(INSTGROUP) $(PROGS) $(DESTDIR)$(BINDIR)
$(INSTALL) -o $(INSTOWNER) -m $(INSTDOCS) $(PHPSCRIPTS) $(DESTDIR)$(WWW_PAGES) $(INSTALL) -m $(INSTMODE) -o $(INSTOWNER) -g $(INSTGROUP) $(PHPSCRIPTS) $(DESTDIR)$(BINDIR)
$(INSTALL) -o $(INSTOWNER) -m $(INSTDOCS) $(PHPWEB) $(DESTDIR)$(WWW_PAGES)
$(INSTALL) $(OWN)-d $(DESTDIR)$(VARHTML)
$(LN) -sf $(VARHTML) $(DESTDIR)$(WWW_PAGES)var
$(INSTALL) $(OWN)-d $(DESTDIR)$(CONFDIR)
$(LN) -sf $(CONFDIR) $(DESTDIR)$(WWW_PAGES)etc
clean: clean:
rm -rf $(PROGS) *.o *~ rm -rf $(PROGS) *.o *~
......
...@@ -49,8 +49,8 @@ ...@@ -49,8 +49,8 @@
//#define EXIF_DEV "/dev/exif_meta" //#define EXIF_DEV "/dev/exif_meta"
#define PROGRAM_SERIAL 0 #define PROGRAM_SERIAL 0
//#define D(x) #define D(x)
#define D(x) x //#define D(x) x
// TODO: use only existing sensor channels? // TODO: use only existing sensor channels?
const char *exif_paths[SENSOR_PORTS] = { const char *exif_paths[SENSOR_PORTS] = {
...@@ -345,7 +345,37 @@ void process_GPGGA(char*response, struct meta_GPSInfo_t *meta) { ...@@ -345,7 +345,37 @@ void process_GPGGA(char*response, struct meta_GPSInfo_t *meta) {
/// Skip 14 = Diff. reference station ID# /// Skip 14 = Diff. reference station ID#
} }
void debug_meta(struct meta_GPSInfo_t *meta)
{
unsigned char * buf = (unsigned char *) meta;
int i;
printf("GPSLatitudeRef %c)\n", meta->GPSLatitudeRef);
printf("GPSLatitude_deg_nom 0x%x)\n",__be32_to_cpu(meta->GPSLatitude_deg_nom));
printf("GPSLatitude_deg_denom 0x%x)\n",__be32_to_cpu(meta->GPSLatitude_deg_denom));
printf("GPSLatitude_min_nom 0x%x)\n",__be32_to_cpu(meta->GPSLatitude_min_nom));
printf("GPSLatitude_min_denom 0x%x)\n",__be32_to_cpu(meta->GPSLatitude_min_denom));
printf("GPSLongitudeRef %c)\n", meta->GPSLongitudeRef);
printf("GPSLongitude_deg_nom 0x%x)\n",__be32_to_cpu(meta->GPSLongitude_deg_nom));
printf("GPSLongitude_deg_denom 0x%x)\n",__be32_to_cpu(meta->GPSLongitude_deg_denom));
printf("GPSLongitude_min_nom 0x%x)\n",__be32_to_cpu(meta->GPSLongitude_min_nom));
printf("GPSLongitude_min_denom 0x%x)\n",__be32_to_cpu(meta->GPSLongitude_min_denom));
printf("GPSAltitudeRef %c)\n", meta->GPSAltitudeRef);
printf("GPSAltitude_nom 0x%x)\n",__be32_to_cpu(meta->GPSAltitude_nom));
printf("GPSAltitude_denom 0x%x)\n",__be32_to_cpu(meta->GPSAltitude_denom));
printf("GPSTimeStamp_hrs_nom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_hrs_nom));
printf("GPSTimeStamp_hrs_denom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_hrs_denom));
printf("GPSTimeStamp_min_nom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_min_nom));
printf("GPSTimeStamp_min_denom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_min_denom));
printf("GPSTimeStamp_sec_nom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_sec_nom));
printf("GPSTimeStamp_sec_denom 0x%x)\n",__be32_to_cpu(meta->GPSTimeStamp_sec_denom));
printf("GPSDateStamp %s)\n", meta->GPSDateStamp);
printf("GPSMeasureMode %c)", meta->GPSMeasureMode);
for (i = 0; i < sizeof (struct meta_GPSInfo_t); i++){
if ((i & 0xf) == 0) printf("\n%02x:",i);
printf(" %02x",(int) buf[i]);
}
printf("\n");
}
...@@ -373,8 +403,8 @@ int main(int argc, char *argv[]) { ...@@ -373,8 +403,8 @@ int main(int argc, char *argv[]) {
fd_exif[chn] = open(exif_paths[chn], O_RDWR); fd_exif[chn] = open(exif_paths[chn], O_RDWR);
if (fd_exif[chn] < 0) { if (fd_exif[chn] < 0) {
fprintf(stderr,"Can not open device file %s\n",exif_paths[chn]); fprintf(stderr,"Can not open device file %s\n",exif_paths[chn]);
} } else good_chns++;
} else good_chns++; }
} }
if (!good_chns){ if (!good_chns){
fprintf(stderr,"Could not open any of EXIF channel device files, aborting\n"); fprintf(stderr,"Could not open any of EXIF channel device files, aborting\n");
...@@ -434,14 +464,19 @@ int main(int argc, char *argv[]) { ...@@ -434,14 +464,19 @@ int main(int argc, char *argv[]) {
// D(printf ("Got GPRMC\n")); // D(printf ("Got GPRMC\n"));
D(printf ("Got NMEA string >>%s<<\n",response+1)); D(printf ("Got NMEA string >>%s<<\n",response+1));
process_GPRMC(response, &meta); process_GPRMC(response, &meta);
D(printf ("sizeof(meta)=0x%x\n",sizeof(meta)));
D(debug_meta(&meta));
for (chn =0; chn < SENSOR_PORTS; chn++) if (fd_exif[chn]>=0){ for (chn =0; chn < SENSOR_PORTS; chn++) if (fd_exif[chn]>=0){
lseek (fd_exif[chn],Exif_GPSInfo_GPSLatitudeRef,SEEK_END); /// position file pointer at the beginning of the data field for GPSLatitudeRef lseek (fd_exif[chn],Exif_GPSInfo_GPSLatitudeRef,SEEK_END); /// position file pointer at the beginning of the data field for GPSLatitudeRef
D(printf ("filepos(%d)=0x%x\n",chn, (int)lseek (fd_exif[chn],0,SEEK_CUR)));
write (fd_exif[chn], &meta, sizeof(meta)); write (fd_exif[chn], &meta, sizeof(meta));
} }
} else if (!memcmp(response+1,"GPGGA", sizeof("GPGGA")-1)) { } else if (!memcmp(response+1,"GPGGA", sizeof("GPGGA")-1)) {
// D(printf ("Got GPRMC\n")); // D(printf ("Got GPRMC\n"));
D(printf ("Got NMEA string >>%s<<\n",response+1)); D(printf ("Got NMEA string >>%s<<\n",response+1));
process_GPGGA(response, &meta); process_GPGGA(response, &meta);
D(printf ("sizeof(meta)=0x%x\n",sizeof(meta)));
D(debug_meta(&meta));
for (chn =0; chn < SENSOR_PORTS; chn++) if (fd_exif[chn]>=0){ for (chn =0; chn < SENSOR_PORTS; chn++) if (fd_exif[chn]>=0){
lseek (fd_exif[chn],Exif_GPSInfo_GPSLatitudeRef,SEEK_END); /// position file pointer at the beginning of the data field for GPSLatitudeRef lseek (fd_exif[chn],Exif_GPSInfo_GPSLatitudeRef,SEEK_END); /// position file pointer at the beginning of the data field for GPSLatitudeRef
write (fd_exif[chn], &meta, sizeof(meta)); write (fd_exif[chn], &meta, sizeof(meta));
......
#!/usr/bin/env php -q #!/usr/bin/env php
<?php <?php
/* /*
*! FILE NAME : start_gps_compass.php *! FILE NAME : start_gps_compass.php
...@@ -51,547 +51,627 @@ ...@@ -51,547 +51,627 @@
//look uptime, sleep if just started (no USB yet) //look uptime, sleep if just started (no USB yet)
//require '/usr/html/includes/i2c.inc'; //require '/usr/html/includes/i2c.inc';
#require 'i2c.inc'; #require 'i2c.inc';
set_include_path(get_include_path() . PATH_SEPARATOR . '/www/pages/include'); set_include_path ( get_include_path () . PATH_SEPARATOR . '/www/pages/include' );
require 'i2c_include.php'; require 'i2c_include.php';
$config_name = "/etc/elphel393/imu_logger.xml"; $config_name = '/etc/elphel393/imu_logger.xml';
$gps_compass_xml = "/www/pages/gps_compass.xml"; $gps_compass_xml = '/www/pages/gps_compass.xml';
$i2cbus = 4; $tmp_config = '/var/volatile/html/logger_config.xml';
$ids=scanGrandDaughters(); $exif_php = "/www/pages/exif.php";
$b_index=indexGrandDaughters($ids); $bindir = '/usr/bin/';
// print_r($ids); $i2cbus = 4;
// print_r($b_index); $ids = scanGrandDaughters ();
$baud=null; $b_index = indexGrandDaughters ( $ids );
if (isset($b_index[103696])) { // print_r($ids);
$baud=$ids[$b_index[103696]]['baud']; // print_r($b_index);
if ($baud===0) $baud=='auto'; $baud = null;
} if (isset ( $b_index [103696] )) {
$baud = $ids [$b_index [103696]] ['baud'];
if ($baud === 0)
$baud == 'auto';
}
// exit (0);
$wait_time = 20; // wait for $wait_time if both devices were not found, retry
$compass = null;
$GPS = null;
$verbose = true;
foreach ( $_SERVER ['argv'] as $param ) {
if ((substr ( $param, 0, 2 ) == "-q") || (substr ( $param, 0, 8 ) == "--silent")) {
$verbose = false;
}
}
// / kill those GPS and compass if they are running already
exec ( 'killall -q garminusb2exif' );
exec ( 'killall -q nmea2exif' );
exec ( 'killall -q compass' );
// find_gps_compass();
$devs = find_gps_compass ( $baud );
// echo "compass: ";var_dump($compass);echo "\n";
// echo "GPS: ";var_dump($GPS);echo "\n";
// if ((!$compass || !$GPS) && ((exec('date +%s')+0) <30)) {
if ((count ( $devs ) == 0) && ((exec ( 'date +%s' ) + 0) < 30)) {
if ($verbose)
echo "waiting $wait_time sec for USB devices to start up\n";
sleep ( $wait_time );
if ($verbose)
echo "Retrying detection\n";
$devs = find_gps_compass ( $baud );
}
listSensorDevices ( $devs, $b_index, $ids );
// var_dump($GPS);
// if (file_exists ($sensor_state_file)) $sensor_board=parse_ini_file($sensor_state_file);
// exec ('sync');
$logger_config = null;
$write_config = false;
if (file_exists ( $config_name )) {
if ($verbose)
echo 'Reading IMU logger configuration from ' . $config_name . "\n";
$config_xml = simplexml_load_file ( $config_name );
$logger_config = loggerConfigFromXML ( $config_xml );
} else if (isset ( $b_index [103696] ) || isset ( $b_index [103696] )) {
if ($verbose)
echo "Generating IMU logger configuration\n";
// $default_config=init_default_config();
// $logger_config=init_default_config();
$logger_config = array ();
if (isset ( $GPS ) && isset ( $b_index [103696] )) {
$logger_config ['baud_rate'] = $GPS ['baud'];
$logger_config ['gps_slot'] = $ids [$b_index [103696]] ['port'];
$logger_config ['gps_mode'] = $ids [$b_index [103696]] ['sync'];
}
if (isset ( $b_index [103695] )) {
$logger_config ['imu_slot'] = $ids [$b_index [103695]] ['port'];
$logger_config ['imu_sa'] = $b_index [103695];
}
$write_config = true; // write configuration to /etc after applying defaults
}
if (isset ( $logger_config )) {
// var_dump($logger_config);
$logger_config = combineLoggerConfigs ( $logger_config, init_default_config () );
// var_dump($logger_config);
// var_dump($default_config);
$logger_xml = loggerConfigToXML ( $logger_config );
$conf_file = fopen ( $tmp_config, "w" );
fwrite ( $conf_file, $logger_xml->asXML () );
fclose ( $conf_file );
setup_IMU_logger ( $logger_config );
if ($write_config) {
if ($verbose)
echo 'Writing IMU logger configuration to ' . $config_name . "\n";
$conf_file = fopen ( $config_name, "w" );
fwrite ( $conf_file, $logger_xml->asXML () );
fclose ( $conf_file );
exec ( 'sync' );
}
}
// echo "compass: ";var_dump($compass);echo "\n";
// echo "GPS: ";var_dump($GPS);echo "\n";
$noGPS = ($compass || $GPS) ? "" : "noGPS";
$nocompass = ($compass) ? "" : "nocompass";
// exit (0);
$wait_time=20; // wait for $wait_time if both devices were not found, retry
$compass=null;
$GPS=null;
$verbose=true;
foreach ($_SERVER['argv'] as $param) {
if ((substr($param,0,2)=="-q") || (substr($param,0,8)=="--silent")){
$verbose=false;
}
}
/// kill those GPS and compass if they are running already // Just debugging:
exec('killall -q garminusb2exif'); //$nocompass = "";
exec('killall -q nmea2exif');
exec('killall -q compass');
// find_gps_compass();
$devs=find_gps_compass($baud);
// echo "compass: ";var_dump($compass);echo "\n";
// echo "GPS: ";var_dump($GPS);echo "\n";
// if ((!$compass || !$GPS) && ((exec('date +%s')+0) <30)) {
if ((count($devs)==0) && ((exec('date +%s')+0) <30)) {
if ($verbose) echo "waiting $wait_time sec for USB devices to start up\n";
sleep($wait_time);
if ($verbose) echo "Retrying detection\n";
$devs=find_gps_compass($baud);
}
listSensorDevices($devs,$b_index,$ids);
//var_dump($GPS);
// if (file_exists ($sensor_state_file)) $sensor_board=parse_ini_file($sensor_state_file);
// exec ('sync');
$logger_config=null;
$write_config=false;
if (file_exists ($config_name)) {
if ($verbose) echo 'Reading IMU logger configuration from '. $config_name."\n";
$config_xml = simplexml_load_file($config_name);
$logger_config=loggerConfigFromXML($config_xml);
} else if (isset($b_index[103696]) || isset($b_index[103696])) {
if ($verbose) echo "Generating IMU logger configuration\n";
// $default_config=init_default_config();
// $logger_config=init_default_config();
$logger_config=array();
if (isset($GPS) && isset($b_index[103696])) {
$logger_config['baud_rate']=$GPS['baud'];
$logger_config['gps_slot']=$ids[$b_index[103696]]['port'];
$logger_config['gps_mode']=$ids[$b_index[103696]]['sync'];
}
if (isset($b_index[103695])) {
$logger_config['imu_slot']=$ids[$b_index[103695]]['port'];
$logger_config['imu_sa']=$b_index[103695];
}
$write_config=true; // write configuration to /etc after applying defaults
}
if (isset($logger_config)){
// var_dump($logger_config);
$logger_config=combineLoggerConfigs($logger_config,init_default_config());
// var_dump($logger_config);
// var_dump($default_config);
$logger_xml=loggerConfigToXML($logger_config);
$conf_file=fopen('/var/html/logger_config.xml',"w");
fwrite($conf_file,$logger_xml->asXML());
fclose($conf_file);
setup_IMU_logger($logger_config);
if ($write_config) {
if ($verbose) echo 'Writing IMU logger configuration to '. $config_name."\n";
$conf_file=fopen($config_name,"w");
fwrite($conf_file,$logger_xml->asXML());
fclose($conf_file);
exec ('sync');
}
}
// echo "compass: ";var_dump($compass);echo "\n"; $cmd = $exif_php." init $noGPS $nocompass";
// echo "GPS: ";var_dump($GPS);echo "\n"; if ($verbose)
$noGPS= ($compass || $GPS)?"" : "noGPS"; echo "Initializing Exif template: $cmd\n";
$nocompass=($compass)?"" : "nocompass"; exec ( $cmd );
$cmd="/usr/html/exif.php init $noGPS $nocompass"; if ($GPS) {
if ($verbose) echo "Initializing Exif template: $cmd\n"; if ($verbose)
exec($cmd); echo "Starting " . $GPS ["name"] . " as " . $GPS ["file"] . "\n";
if ($GPS) { if (strpos ( $GPS ['name'], 'NMEA' ) !== false) {
if ($verbose) echo "Starting ".$GPS["name"]. " as ". $GPS["file"]. "\n"; $cmd = $bindir."nmea2exif " . $GPS ["file"] . " &";
if (strpos($GPS['name'],'NMEA')!==false) { } else {
$cmd ="/usr/local/bin/nmea2exif ". $GPS["file"]." &"; $cmd = $bindir."garminusb2exif " . $GPS ["file"] . " &"; // BUG: Not ported!
} else { }
$cmd ="/usr/local/bin/garminusb2exif ". $GPS["file"]." &"; if ($verbose)
} echo "exec: $cmd \n";
if ($verbose) echo "exec: $cmd \n"; popen ( $cmd, "r" );
popen($cmd,"r"); }
} if ($compass) {
if ($compass) { if ($verbose)
if ($verbose) echo "Starting ".$compass["name"]. " as ". $compass["file"]. "\n"; echo "Starting " . $compass ["name"] . " as " . $compass ["file"] . "\n";
exec ("stty -F ".$compass["file"]." -echo speed 19200"); exec ( "stty -F " . $compass ["file"] . " -echo speed 19200" );
$cmd="/usr/local/bin/compass ". $compass["file"]." &"; $cmd = $bindir."compass " . $compass ["file"] . " &";
if ($verbose) echo "exec: $cmd \n"; if ($verbose)
popen($cmd,"r"); echo "exec: $cmd \n";
} popen ( $cmd, "r" );
exit ((isset($b_index[103695])?4:0) | ($compass?2:0) | ($GPS?1:0)); }
function scanGrandDaughters(){ exit ( (isset ( $b_index [103695] ) ? 4 : 0) | ($compass ? 2 : 0) | ($GPS ? 1 : 0) );
function scanGrandDaughters() {
global $i2cbus; global $i2cbus;
$ids=array(); $ids = array ();
$id=""; $id = "";
for ($i=2;$i<8;$i++) { for($i = 2; $i < 8; $i ++) {
$id=i2c_read256b(0xa0+2*$i,$i2cbus); $id = i2c_read256b ( 0xa0 + 2 * $i, $i2cbus );
$xml=simplexml_load_string($id); $xml = simplexml_load_string ( $id );
if (($xml!==false) && ($xml->getName()=='board')) { if (($xml !== false) && ($xml->getName () == 'board')) {
$ids[$i]=array(); $ids [$i] = array ();
foreach ($xml->children() as $entry) { foreach ( $xml->children () as $entry ) {
$ids[$i][$entry->getName()]=(string) $entry ; $ids [$i] [$entry->getName ()] = ( string ) $entry;
} }
} }
} }
return $ids; return $ids;
} }
function indexGrandDaughters($ids) { function indexGrandDaughters($ids) {
$index=array(); $index = array ();
foreach($ids as $i=>$id) if (isset($id['model'])) { foreach ( $ids as $i => $id )
$index[$id['model']]=$i; if (isset ( $id ['model'] )) {
} $index [$id ['model']] = $i;
return $index; }
return $index;
} }
/// compass/GPS programs should be killed before detection // / compass/GPS programs should be killed before detection
// baud=null - search USB Garmin and compass // baud=null - search USB Garmin and compass
// baud=auto - try to autodetect baud rate // baud=auto - try to autodetect baud rate
// baud=number - used specified baud rate, do not wait for the GPS response // baud=number - used specified baud rate, do not wait for the GPS response
function find_gps_compass($baud=null,$timeout=5) { function find_gps_compass($baud = null, $timeout = 5) {
global $verbose; global $verbose;
$ttyOptions=array ( $ttyOptions = array (
'-echo speed 115200', '-echo speed 115200',
'-echo speed 57600', '-echo speed 57600',
'-echo speed 56000', '-echo speed 56000',
'-echo speed 38400', '-echo speed 38400',
'-echo speed 19200', /// maybe other options, like '-echo speed 4800 -ixoff' '-echo speed 19200', // / maybe other options, like '-echo speed 4800 -ixoff'
'-echo speed 9600', '-echo speed 9600',
'-echo speed 4800', '-echo speed 4800',
'-echo speed 2400' '-echo speed 2400'
); );
if (isset($baud) && ($baud!='auto') && ($baud!=0)) { if (isset ( $baud ) && ($baud != 'auto') && ($baud != 0)) {
$ttyOptions=array ( $ttyOptions = array (
'-echo speed '.$baud); '-echo speed ' . $baud
} );
// global $GPS, $compass; }
$devices=array( // global $GPS, $compass;
"compass"=>array(array("name"=>"Ocean Server OS-5000","driver"=>"cp2101")), $devices = array (
"GPS" =>array(array("name"=>"Garmin GPS 18 USB","driver"=>"garmin_gps"))); "compass" => array (
exec("ls /sys/bus/usb-serial/devices",$usb_ser_devs); // ("ttyUSB0","ttyUSB1") array (
$devs = new SimpleXMLElement("<?xml version='1.0' standalone='yes'?><USB_serial_devices/>"); "name" => "Ocean Server OS-5000",
$serialDevices=array(); "driver" => "cp2101"
$compassPresent=false; )
$gpsPresent=false; ),
$driver=array(); "GPS" => array (
//echo "usb_ser_devs"; var_dump($usb_ser_devs); array (
foreach ($usb_ser_devs as $dev) { "name" => "Garmin GPS 18 USB",
//echo "dev=$dev\n"; "driver" => "garmin_gps"
$arr=split("/",exec("ls /sys/bus/usb-serial/devices/".$dev."/driver -l")); )
$driver[$dev]=$arr[count($arr)-1]; )
$serialDevices[$dev]=''; );
if (!isset($baud)) { // if baud is set (auto or number - skip compass, it is 103696) exec ( "ls /sys/bus/usb-serial/devices", $usb_ser_devs ); // ("ttyUSB0","ttyUSB1")
foreach ($devices["compass"] as $d) { $devs = new SimpleXMLElement ( "<?xml version='1.0' standalone='yes'?><USB_serial_devices/>" );
if ($d["driver"]==$driver[$dev]) { $serialDevices = array ();
$dd=$devs->addChild ('compass'); $compassPresent = false;
$dd->addChild ('name',$d["name"]); $gpsPresent = false;
$dd->addChild ('driver',$d["driver"]); $driver = array ();
$dd->addChild ('file',"/dev/".$dev); // echo "usb_ser_devs"; var_dump($usb_ser_devs);
$serialDevices[$dev]='compass'; foreach ( $usb_ser_devs as $dev ) {
$compassPresent=true; // echo "dev=$dev\n";
} $arr = split ( "/", exec ( "ls /sys/bus/usb-serial/devices/" . $dev . "/driver -l" ) );
} $driver [$dev] = $arr [count ( $arr ) - 1];
} $serialDevices [$dev] = '';
if ($serialDevices[$dev]) continue; if (! isset ( $baud )) { // if baud is set (auto or number - skip compass, it is 103696)
foreach ($devices["GPS"] as $d) { foreach ( $devices ["compass"] as $d ) {
if ($d["driver"]==$driver[$dev]) { if ($d ["driver"] == $driver [$dev]) {
$dd=$devs->addChild ('GPS'); $dd = $devs->addChild ( 'compass' );
$dd->addChild ('name',$d["name"]); $dd->addChild ( 'name', $d ["name"] );
$dd->addChild ('driver',$d["driver"]); $dd->addChild ( 'driver', $d ["driver"] );
$dd->addChild ('file',"/dev/".$dev); $dd->addChild ( 'file', "/dev/" . $dev );
$serialDevices[$dev]='gps'; $serialDevices [$dev] = 'compass';
$gpsPresent=true; $compassPresent = true;
} }
} }
} }
/// Do we need to look for a NMEA GPS? if ($serialDevices [$dev])
if (!$gpsPresent) foreach ($serialDevices as $dev=>$type) if (!$type && !$gpsPresent) { continue;
if ($verbose) printf ("could be $dev\n"); foreach ( $devices ["GPS"] as $d ) {
foreach ($ttyOptions as $ttyOpt) { if ($d ["driver"] == $driver [$dev]) {
$cmd='stty -F /dev/'.$dev.' '.$ttyOpt; $dd = $devs->addChild ( 'GPS' );
if (count($ttyOptions)==1) { /// single baud option, will not wait for confirmation. Has to be only one serial adapter !! $dd->addChild ( 'name', $d ["name"] );
if ($verbose) echo "Setting: $cmd\n"; $dd->addChild ( 'driver', $d ["driver"] );
exec ($cmd); $dd->addChild ( 'file', "/dev/" . $dev );
$gpsPresent=true; $serialDevices [$dev] = 'gps';
} else { $gpsPresent = true;
if ($verbose) echo "Trying: $cmd\n"; }
exec ($cmd); }
unset ($fullOutput); }
exec ('timeout '.$timeout.' cat /dev/'.$dev, $fullOutput); // &$fullOutput - Fatal error: Call-time pass-by-reference has been removed in php // / Do we need to look for a NMEA GPS?
//var_dump($fullOutput); if (! $gpsPresent)
foreach ($fullOutput as $line) if (strpos ($line, '$GP')===0) { foreach ( $serialDevices as $dev => $type )
$gpsPresent=true; if (! $type && ! $gpsPresent) {
break; if ($verbose)
} printf ( "could be $dev\n" );
} foreach ( $ttyOptions as $ttyOpt ) {
if ($gpsPresent) { $cmd = 'stty -F /dev/' . $dev . ' ' . $ttyOpt;
$dd=$devs->addChild ('GPS'); if (count ( $ttyOptions ) == 1) { // / single baud option, will not wait for confirmation. Has to be only one serial adapter !!
$dd->addChild ('name','NMEA 0183 GPS receiver'); if ($verbose)
$dd->addChild ('driver',$driver[$dev]); echo "Setting: $cmd\n";
$dd->addChild ('file',"/dev/".$dev); exec ( $cmd );
$arr=split(" ",$ttyOpt); $gpsPresent = true;
$dd->addChild ('baud',$arr[count($arr)-1]); } else {
$serialDevices[$dev]='gps'; if ($verbose)
if ($verbose) echo "\nfound NMEA GPS unit\n"; echo "Trying: $cmd\n";
break; exec ( $cmd );
} unset ( $fullOutput );
// var_dump($fullOutput); exec ( 'timeout ' . $timeout . ' cat /dev/' . $dev, $fullOutput ); // &$fullOutput - Fatal error: Call-time pass-by-reference has been removed in php
} // var_dump($fullOutput);
} foreach ( $fullOutput as $line )
/// list unused USB serial adapters if (strpos ( $line, '$GP' ) === 0) {
foreach ($serialDevices as $dev=>$type) if (!$type) { $gpsPresent = true;
$dd=$devs->addChild ('unused'); break;
$dd->addChild ('name','USB serial converter'); }
$dd->addChild ('driver',$driver[$dev]); }
$dd->addChild ('file',"/dev/".$dev); if ($gpsPresent) {
} $dd = $devs->addChild ( 'GPS' );
return $devs; $dd->addChild ( 'name', 'NMEA 0183 GPS receiver' );
$dd->addChild ( 'driver', $driver [$dev] );
$dd->addChild ( 'file', "/dev/" . $dev );
$arr = split ( " ", $ttyOpt );
$dd->addChild ( 'baud', $arr [count ( $arr ) - 1] );
$serialDevices [$dev] = 'gps';
if ($verbose)
echo "\nfound NMEA GPS unit\n";
break;
}
// var_dump($fullOutput);
}
}
// / list unused USB serial adapters
foreach ( $serialDevices as $dev => $type )
if (! $type) {
$dd = $devs->addChild ( 'unused' );
$dd->addChild ( 'name', 'USB serial converter' );
$dd->addChild ( 'driver', $driver [$dev] );
$dd->addChild ( 'file', "/dev/" . $dev );
}
return $devs;
} }
function listSensorDevices($devs,$b_index,$ids) { function listSensorDevices($devs, $b_index, $ids) {
global $GPS, $compass,$gps_compass_xml; global $GPS, $compass, $gps_compass_xml;
foreach ($devs->compass as $a) { foreach ( $devs->compass as $a ) {
$compass=array("file"=>(string) $a->file, "name"=> (string) $a->name); $compass = array (
break; // use first one "file" => ( string ) $a->file,
} "name" => ( string ) $a->name
foreach ($devs->GPS as $a) { );
$GPS=array("file"=>(string) $a->file, "name"=> (string) $a->name, "baud"=>(string) $a->baud); break; // use first one
if (isset($b_index[103696])) { }
$a->addChild ('part_number',$ids[$b_index[103696]]['part']); foreach ( $devs->GPS as $a ) {
$a->addChild ('port',$ids[$b_index[103696]]['port']); $GPS = array (
$a->addChild ('mode',$ids[$b_index[103696]]['mode']); "file" => ( string ) $a->file,
} "name" => ( string ) $a->name,
"baud" => ( string ) $a->baud
break; // use first one );
} if (isset ( $b_index [103696] )) {
$state_file=fopen($gps_compass_xml,"w"); $a->addChild ( 'part_number', $ids [$b_index [103696]] ['part'] );
fwrite($state_file,$devs->asXML()); $a->addChild ( 'port', $ids [$b_index [103696]] ['port'] );
fclose($state_file); $a->addChild ( 'mode', $ids [$b_index [103696]] ['mode'] );
}
break; // use first one
}
$state_file = fopen ( $gps_compass_xml, "w" );
fwrite ( $state_file, $devs->asXML () );
fclose ( $state_file );
} }
function loggerConfigToXML($conf){ function loggerConfigToXML($conf) {
$logger_xml = new SimpleXMLElement("<?xml version='1.0' standalone='yes'?><Logger_configuration/>"); $logger_xml = new SimpleXMLElement ( "<?xml version='1.0' standalone='yes'?><Logger_configuration/>" );
foreach ($conf as $key=>$value) { foreach ( $conf as $key => $value ) {
switch ($key) { switch ($key) {
case 'imu_registers': case 'imu_registers' :
$subtree=$logger_xml->addChild ($key); $subtree = $logger_xml->addChild ( $key );
for ($i=0;$i<count($value);$i++) { for($i = 0; $i < count ( $value ); $i ++) {
$subtree->addChild ('R_'.sprintf('%02d',$i),sprintf("0x%02x",$value[$i])); $subtree->addChild ( 'R_' . sprintf ( '%02d', $i ), sprintf ( "0x%02x", $value [$i] ) );
} }
break; break;
case 'nmea': case 'nmea' :
$subtree=$logger_xml->addChild ($key); $subtree = $logger_xml->addChild ( $key );
for ($i=0;$i<count($value);$i++) { for($i = 0; $i < count ( $value ); $i ++) {
$sentence=$subtree->addChild ('S_'.sprintf('%01d',$i)); $sentence = $subtree->addChild ( 'S_' . sprintf ( '%01d', $i ) );
$sentence->addChild ('sentence',$value[$i][0]); $sentence->addChild ( 'sentence', $value [$i] [0] );
$sentence->addChild ('format',$value[$i][1]); $sentence->addChild ( 'format', $value [$i] [1] );
} }
break; break;
default: default :
$logger_xml->addChild ($key,$value); $logger_xml->addChild ( $key, $value );
} }
} }
return $logger_xml; return $logger_xml;
} }
function loggerConfigFromXML($confXML){ function loggerConfigFromXML($confXML) {
$conf=array(); $conf = array ();
// var_dump($confXML); // var_dump($confXML);
foreach ($confXML as $item) { foreach ( $confXML as $item ) {
// var_dump($item); // var_dump($item);
$key=$item->getName(); $key = $item->getName ();
switch ($key) { switch ($key) {
case 'imu_registers': case 'imu_registers' :
$conf[$key]=array(); $conf [$key] = array ();
foreach ($item as $reg) { foreach ( $item as $reg ) {
$rname=substr($reg->getName(),2); $rname = substr ( $reg->getName (), 2 );
$conf[$key][$rname+0]=((string) $reg)+0; $conf [$key] [$rname + 0] = (( string ) $reg) + 0;
} }
break; break;
case 'nmea': case 'nmea' :
$conf[$key]=array(); $conf [$key] = array ();
foreach ($item as $reg) { foreach ( $item as $reg ) {
$rname=substr($reg->getName(),2); $rname = substr ( $reg->getName (), 2 );
$conf[$key][$rname+0]=array((string) $reg->sentence, (string) $reg->format); $conf [$key] [$rname + 0] = array (
} ( string ) $reg->sentence,
break; ( string ) $reg->format
case 'imu_period': );
$conf[$key]=(string) $item; }
if ($conf[$key]!='auto') $conf[$key]+=0; break;
break; case 'imu_period' :
case 'message': $conf [$key] = ( string ) $item;
$conf[$key]=(string) $item; if ($conf [$key] != 'auto')
break; $conf [$key] += 0;
default: break;
// $conf[$key]=(int) $item; case 'message' :
$conf[$key]=((string) $item)+0; $conf [$key] = ( string ) $item;
} break;
} default :
return $conf; // $conf[$key]=(int) $item;
$conf [$key] = (( string ) $item) + 0;
}
}
return $conf;
} }
function init_default_config() {
$default_config = array (
'imu_period' => 'auto', // 0xFFFFFFFF, // 0 - off, >=0xff000000 - "when ready", else - number of SCLK periods
function init_default_config(){ 'sclk_freq' => 5000000, // $clkr_divr= 8, // 80MHz divisor to get half SCLK rate (defaulkt SCLK=5MHz)
$default_config=array( 'stall' => 2, // SPI stall time in usec
'imu_period'=>'auto',// 0xFFFFFFFF, // 0 - off, >=0xff000000 - "when ready", else - number of SCLK periods 'baud_rate' => 19200, // $rs232_div=80/2/$baud
'sclk_freq'=>5000000, // $clkr_divr= 8, // 80MHz divisor to get half SCLK rate (defaulkt SCLK=5MHz) 'imu_slot' => 1,
'stall' => 2, // SPI stall time in usec 'gps_slot' => 2,
'baud_rate'=>19200, // $rs232_div=80/2/$baud 'gps_mode' => 2, // 0 - pps pos, 1 - pps neg, 2 - start of first sesntence after pause, 3 start of sentence
'imu_slot'=>1, 'msg_conf' => 10, // GPIO bit number for external (odometer) input (+16 - invert polarity). Timestamp uses leading edge, software may write to 56-byte buffer before the trailing edge
'gps_slot'=>2, 'img_sync' => 1, // enable logging image acquisition starts (0 - disable)
'gps_mode'=>2, // 0 - pps pos, 1 - pps neg, 2 - start of first sesntence after pause, 3 start of sentence 'extra_conf' => 4, // 0 - config_long_sda_en, 1 -config_late_clk, 2 - config_single_wire, should be set for 103695 rev "A"
'msg_conf'=>10, // GPIO bit number for external (odometer) input (+16 - invert polarity). Timestamp uses leading edge, software may write to 56-byte buffer before the trailing edge 'slow_spi' => 0, // set to 1 for slow SPI devices (0 for ADIS-16375)
'img_sync'=>1, // enable logging image acquisition starts (0 - disable) 'imu_sa' => 3, // i2c slave address modifier for the 103695A pca9500
'extra_conf'=>4, // 0 - config_long_sda_en, 1 -config_late_clk, 2 - config_single_wire, should be set for 103695 rev "A" 'sleep_busy' => 30000, // microseconds
'slow_spi'=>0, // set to 1 for slow SPI devices (0 for ADIS-16375) 'imu_registers' => array ( // / Up to 28 total
'imu_sa'=>3, // i2c slave address modifier for the 103695A pca9500 0x10, // x gyro low
'sleep_busy'=>30000, //microseconds 0x12, // x gyro high
'imu_registers'=>array( /// Up to 28 total 0x14, // y gyro low
0x10, // x gyro low 0x16, // y gyro high
0x12, // x gyro high 0x18, // z gyro low
0x14, // y gyro low 0x1a, // z gyro high
0x16, // y gyro high
0x18, // z gyro low 0x1c, // x accel low
0x1a, // z gyro high 0x1e, // x accel high
0x20, // y accel low
0x1c, // x accel low 0x22, // y accel high
0x1e, // x accel high 0x24, // z accel low
0x20, // y accel low 0x26, // z accel high
0x22, // y accel high
0x24, // z accel low 0x40, // x delta angle low
0x26, // z accel high 0x42, // x delta angle high
0x44, // y delta angle low
0x40, // x delta angle low 0x46, // y delta angle high
0x42, // x delta angle high 0x48, // z delta angle low
0x44, // y delta angle low 0x4a, // z delta angle high
0x46, // y delta angle high
0x48, // z delta angle low 0x4c, // x delta velocity low
0x4a, // z delta angle high 0x4e, // x delta velocity high
0x50, // y delta velocity low
0x4c, // x delta velocity low 0x52, // y delta velocity high
0x4e, // x delta velocity high 0x54, // z delta velocity low
0x50, // y delta velocity low 0x56, // z delta velocity high
0x52, // y delta velocity high
0x54, // z delta velocity low 0x0e, // temperature
0x56, // z delta velocity high 0x70, // time m/s
0x72, // time d/h
0x0e, // temperature 0x74
0x70, // time m/s ) // time y/m
0x72, // time d/h ,
0x74// time y/m 'nmea' => array (
), // / first three letters - sentence to log (letters after "$GP"). next "n"/"b" (up to 24 total) - "n" number (will be encoded 4 digits/byte, follwed by "0xF"
'nmea'=>array( // / "b" - byte - all but last will have MSB 0 (& 0x7f), the last one - with MSB set (| 0x80). If there are no characters in the field 0xff will be output
/// first three letters - sentence to log (letters after "$GP"). next "n"/"b" (up to 24 total) - "n" number (will be encoded 4 digits/byte, follwed by "0xF" array (
/// "b" - byte - all but last will have MSB 0 (& 0x7f), the last one - with MSB set (| 0x80). If there are no characters in the field 0xff will be output 'RMC',
array('RMC','nbnbnbnnnnb'), 'nbnbnbnnnnb'
array('GGA','nnbnbnnnnbnbbb'), ),
array('GSA','bnnnnnnnnnnnnnnnn'), array (
array('VTG','nbnbnbnb') 'GGA',
), 'nnbnbnnnnbnbbb'
'message'=>'Odometer message' // Message - up to 56 bytes ),
); array (
return $default_config; 'GSA',
'bnnnnnnnnnnnnnnnn'
),
array (
'VTG',
'nbnbnbnb'
)
),
'message' => 'Odometer message'
) // Message - up to 56 bytes
;
return $default_config;
} }
/** /**
Combines $conf and $dflt arrays - all driver parameters are overwritten aftre the call * Combines $conf and $dflt arrays - all driver parameters are overwritten aftre the call
*/ */
function combineLoggerConfigs($new_conf,$dflt=null){ function combineLoggerConfigs($new_conf, $dflt = null) {
global $default_config; global $default_config;
// $xclk_freq=80000000; // 80 MHz in NC353 // $xclk_freq=80000000; // 80 MHz in NC353
$xclk_freq=100000000; // 100 MHz in NC393 $xclk_freq = 100000000; // 100 MHz in NC393
$data=array(); $data = array ();
$index=0; $index = 0;
if (!isset ($dflt)) { if (! isset ( $dflt )) {
$dflt=$default_config; $dflt = $default_config;
} }
$conf=$dflt; $conf = $dflt;
if (isset($new_conf)) foreach ($conf as $key=>$oldvalue) if (isset($new_conf[$key])) $conf[$key]=$new_conf[$key]; if (isset ( $new_conf ))
/// fix insufficient data from default foreach ( $conf as $key => $oldvalue )
if (isset ( $new_conf [$key] ))
while (count($conf['imu_registers'])<28) $conf['imu_registers'][count($conf['imu_registers'])]=0; // zero pad if less than 28 registers $conf [$key] = $new_conf [$key];
/// Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults // / fix insufficient data from default
while (count($conf['nmea'])<4) {
for ($i=0;$i<4;$i++) { while ( count ( $conf ['imu_registers'] ) < 28 )
$new=true; $conf ['imu_registers'] [count ( $conf ['imu_registers'] )] = 0; // zero pad if less than 28 registers
for ($j=0;$j<count($conf['nmea']);$j++) if ($conf['nmea'][$j][0]==$dflt['nmea'][$i][0]){ // compare sentence code last 3 letters // / Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults
$new=false; while ( count ( $conf ['nmea'] ) < 4 ) {
break; for($i = 0; $i < 4; $i ++) {
} $new = true;
if ($new) { for($j = 0; $j < count ( $conf ['nmea'] ); $j ++)
$conf['nmea'][count($conf['nmea'])]=$dflt['nmea'][$i]; if ($conf ['nmea'] [$j] [0] == $dflt ['nmea'] [$i] [0]) { // compare sentence code last 3 letters
break; $new = false;
} break;
} }
} if ($new) {
for ($i=0;$i<4;$i++) { $conf ['nmea'] [count ( $conf ['nmea'] )] = $dflt ['nmea'] [$i];
while (strlen($conf['nmea'][$i][0])<3) $conf['nmea'][$i][0].="A"; // just to avoid errors - nonexistent sentences will not hurt, just never logged break;
} }
}
return $conf; }
for($i = 0; $i < 4; $i ++) {
while ( strlen ( $conf ['nmea'] [$i] [0] ) < 3 )
$conf ['nmea'] [$i] [0] .= "A"; // just to avoid errors - nonexistent sentences will not hurt, just never logged
}
return $conf;
} }
function setup_IMU_logger($conf){ function setup_IMU_logger($conf) {
global $verbose; global $verbose;
// $xclk_freq=80000000; // 80 MHz in NC353 // $xclk_freq=80000000; // 80 MHz in NC353
$xclk_freq=100000000; // 100 MHz in NC393 $xclk_freq = 100000000; // 100 MHz in NC393
$data=array(); $data = array ();
$index=0; $index = 0;
/// program period // / program period
if (strtolower($conf['imu_period'])=='auto') $conf['imu_period']=0xffffffff; if (strtolower ( $conf ['imu_period'] ) == 'auto')
else $conf['imu_period']+=0; $conf ['imu_period'] = 0xffffffff;
$data[$index++]= $conf['imu_period'] & 0xff; else
$data[$index++]=($conf['imu_period']>> 8) & 0xff; $conf ['imu_period'] += 0;
$data[$index++]=($conf['imu_period']>>16) & 0xff; $data [$index ++] = $conf ['imu_period'] & 0xff;
$data[$index++]=($conf['imu_period']>>24) & 0xff; $data [$index ++] = ($conf ['imu_period'] >> 8) & 0xff;
/// program SCLK divisor and SPI stall time $data [$index ++] = ($conf ['imu_period'] >> 16) & 0xff;
$sclk_div=round($xclk_freq/$conf['sclk_freq']/2); $data [$index ++] = ($conf ['imu_period'] >> 24) & 0xff;
if ($sclk_div<1) $sclk_div=1; // / program SCLK divisor and SPI stall time
else if ($sclk_div>255) $sclk_div=255; $sclk_div = round ( $xclk_freq / $conf ['sclk_freq'] / 2 );
$conf['sclk_freq']=$xclk_freq/$sclk_div/2; if ($sclk_div < 1)
$stall=(( $conf['stall'] * ( $xclk_freq / $sclk_div )) / 1000000 ); $sclk_div = 1;
if ($stall< 1) $stall= 1; /// does 0 work? need to verify else if ($sclk_div > 255)
else if ($stall>255) $stall=255; $sclk_div = 255;
$conf['stall']=$stall * 1000000 / ($xclk_freq / $sclk_div); $conf ['sclk_freq'] = $xclk_freq / $sclk_div / 2;
$data[$index++]= $sclk_div; $stall = (($conf ['stall'] * ($xclk_freq / $sclk_div)) / 1000000);
$data[$index++]= $stall; if ($stall < 1)
$data[$index++]= 0; $stall = 1; // / does 0 work? need to verify
$data[$index++]= 0; else if ($stall > 255)
/// Program rs232 baud rate $stall = 255;
$rs232_div=round($xclk_freq /2/$conf['baud_rate']); $conf ['stall'] = $stall * 1000000 / ($xclk_freq / $sclk_div);
$conf['baud_rate']=$xclk_freq /2/$rs232_div; $data [$index ++] = $sclk_div;
$data[$index++]= $rs232_div & 0xff; $data [$index ++] = $stall;
$data[$index++]=($rs232_div>> 8) & 0xff; $data [$index ++] = 0;
$data[$index++]=($rs232_div>>16) & 0xff; $data [$index ++] = 0;
$data[$index++]=($rs232_div>>24) & 0xff; // / Program rs232 baud rate
/// Program logger configuration (possible to modify only some fields - maybe support it here?) $rs232_div = round ( $xclk_freq / 2 / $conf ['baud_rate'] );
$logger_conf=0; $conf ['baud_rate'] = $xclk_freq / 2 / $rs232_div;
/// #define IMUCR__IMU_SLOT__BITNM 0 // slot, where 103695 (imu) board is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11) $data [$index ++] = $rs232_div & 0xff;
/// #define IMUCR__IMU_SLOT__WIDTH 2 $data [$index ++] = ($rs232_div >> 8) & 0xff;
$logger_conf |= (($conf['imu_slot'] & 0x3) | 0x4 )<< 0; /// "4" (bit 2 set) here means that the data in bits 0,1 will be applied in the FPGA register $data [$index ++] = ($rs232_div >> 16) & 0xff;
$data [$index ++] = ($rs232_div >> 24) & 0xff;
/// #define IMUCR__GPS_CONF__BITNM 3 // slot, where 103695 (imu) bnoard is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11) // / Program logger configuration (possible to modify only some fields - maybe support it here?)
/// #define IMUCR__GPS_CONF__WIDTH 4 // bits 0,1 - slot #, same as for IMU_SLOT, bits 2,3: $logger_conf = 0;
// 0 - ext pulse, leading edge, // / #define IMUCR__IMU_SLOT__BITNM 0 // slot, where 103695 (imu) board is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11)
// 1 - ext pulse, trailing edge // / #define IMUCR__IMU_SLOT__WIDTH 2
// 2 - start of the first rs232 character after pause $logger_conf |= (($conf ['imu_slot'] & 0x3) | 0x4) << 0; // / "4" (bit 2 set) here means that the data in bits 0,1 will be applied in the FPGA register
// 3 - start of the last "$" character (start of each NMEA sentence)
$logger_conf |= ((($conf['gps_slot'] & 0x3) | (($conf['gps_mode'] & 0x3)<<2) ) | 0x10 )<< 3; // / #define IMUCR__GPS_CONF__BITNM 3 // slot, where 103695 (imu) bnoard is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11)
/// #define IMUCR__MSG_CONF__BITNM 8 // source of external pulses to log: // / #define IMUCR__GPS_CONF__WIDTH 4 // bits 0,1 - slot #, same as for IMU_SLOT, bits 2,3:
/// #define IMUCR__MSG_CONF__WIDTH 5 // bits 0-3 - number of fpga GPIO input 0..11 (i.e. 0x0a - external optoisolated sync input (J15) // 0 - ext pulse, leading edge,
// 0x0f - disable MSG module // 1 - ext pulse, trailing edge
// bit 4 - invert polarity: 0 - timestamp leading edge, log at trailing edge, 1 - opposite // 2 - start of the first rs232 character after pause
// software may set (up to 56 bytes) log message before trailing end of the pulse // 3 - start of the last "$" character (start of each NMEA sentence)
$logger_conf |= (($conf['msg_conf'] & 0x1f) | 0x20 )<< 8; $logger_conf |= ((($conf ['gps_slot'] & 0x3) | (($conf ['gps_mode'] & 0x3) << 2)) | 0x10) << 3;
/// #define IMUCR__SYN_CONF__BITNM 14 // logging frame time stamps (may be synchronized by another camera and have timestamp of that camera) // / #define IMUCR__MSG_CONF__BITNM 8 // source of external pulses to log:
/// #define IMUCR__SYN_CONF__WIDTH 1 // 0 - disable, 1 - enable // / #define IMUCR__MSG_CONF__WIDTH 5 // bits 0-3 - number of fpga GPIO input 0..11 (i.e. 0x0a - external optoisolated sync input (J15)
$logger_conf |= (($conf['img_sync'] & 0x1) | 0x2 )<< 14; // 0x0f - disable MSG module
/// #define IMUCR__RST_CONF__BITNM 16 // reset module // bit 4 - invert polarity: 0 - timestamp leading edge, log at trailing edge, 1 - opposite
/// #define IMUCR__RST_CONF__WIDTH 1 // 0 - enable, 1 -reset (needs resettimng DMA address in ETRAX also) // software may set (up to 56 bytes) log message before trailing end of the pulse
$logger_conf |= (($conf ['msg_conf'] & 0x1f) | 0x20) << 8;
/// #define IMUCR__DBG_CONF__BITNM 18 // several extra IMU configuration bits // / #define IMUCR__SYN_CONF__BITNM 14 // logging frame time stamps (may be synchronized by another camera and have timestamp of that camera)
/// #define IMUCR__DBG_CONF__WIDTH 4 // 0 - config_long_sda_en, 1 -config_late_clk, 2 - config_single_wire, should be set for 103695 rev "A" // / #define IMUCR__SYN_CONF__WIDTH 1 // 0 - disable, 1 - enable
$logger_conf |= (($conf['extra_conf'] & 0xf) | 0x10 )<< 18; $logger_conf |= (($conf ['img_sync'] & 0x1) | 0x2) << 14;
/// next bits used by the driver only, not the FPGA // / #define IMUCR__RST_CONF__BITNM 16 // reset module
/// ((SLOW_SPI & 1)<<23) | \ // / #define IMUCR__RST_CONF__WIDTH 1 // 0 - enable, 1 -reset (needs resettimng DMA address in ETRAX also)
/// (DFLT_SLAVE_ADDR << 24))
$logger_conf |= ($conf['slow_spi'] & 0x1)<< 23; // / #define IMUCR__DBG_CONF__BITNM 18 // several extra IMU configuration bits
$logger_conf |= ($conf['imu_sa'] & 0x7)<< 24; // / #define IMUCR__DBG_CONF__WIDTH 4 // 0 - config_long_sda_en, 1 -config_late_clk, 2 - config_single_wire, should be set for 103695 rev "A"
$data[$index++]= $logger_conf & 0xff; $logger_conf |= (($conf ['extra_conf'] & 0xf) | 0x10) << 18;
$data[$index++]=($logger_conf>> 8) & 0xff; // / next bits used by the driver only, not the FPGA
$data[$index++]=($logger_conf>>16) & 0xff; // / ((SLOW_SPI & 1)<<23) | \
$data[$index++]=($logger_conf>>24) & 0xff; // / (DFLT_SLAVE_ADDR << 24))
/// Set time driver will go to sleep if the data is not ready yet (less than full sample in the buffer) $logger_conf |= ($conf ['slow_spi'] & 0x1) << 23;
$data[$index++]= $conf['sleep_busy'] & 0xff; $logger_conf |= ($conf ['imu_sa'] & 0x7) << 24;
$data[$index++]=($conf['sleep_busy']>> 8) & 0xff; $data [$index ++] = $logger_conf & 0xff;
$data[$index++]=($conf['sleep_busy']>>16) & 0xff; $data [$index ++] = ($logger_conf >> 8) & 0xff;
$data[$index++]=($conf['sleep_busy']>>24) & 0xff; $data [$index ++] = ($logger_conf >> 16) & 0xff;
/// Set IMU register addresses to logger $data [$index ++] = ($logger_conf >> 24) & 0xff;
// echo "1\n"; // / Set time driver will go to sleep if the data is not ready yet (less than full sample in the buffer)
for ($i=0;$i<28;$i++) $data[$index++]= $conf['imu_registers'][$i]; // truncate to 28 $data [$index ++] = $conf ['sleep_busy'] & 0xff;
/// Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults $data [$index ++] = ($conf ['sleep_busy'] >> 8) & 0xff;
// echo "2\n"; $data [$index ++] = ($conf ['sleep_busy'] >> 16) & 0xff;
for ($i=0;$i<4;$i++) { $data [$index ++] = ($conf ['sleep_busy'] >> 24) & 0xff;
$d =str_split($conf['nmea'][$i][0]); // / Set IMU register addresses to logger
// print_r($d); // echo "1\n";
for ($j=0;$j<3;$j++) $data[$index++]=ord($d[$j]); for($i = 0; $i < 28; $i ++)
$d =str_split($conf['nmea'][$i][1]); $data [$index ++] = $conf ['imu_registers'] [$i]; // truncate to 28
// print_r($d); // / Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults
for ($j=0;$j<29;$j++) $data[$index++]=($j<count($d))?ord($d[$j]):0; // echo "2\n";
} for($i = 0; $i < 4; $i ++) {
// echo "3\n"; $d = str_split ( $conf ['nmea'] [$i] [0] );
// print_r($d);
/// Set default message for($j = 0; $j < 3; $j ++)
$d =str_split($conf['message']); $data [$index ++] = ord ( $d [$j] );
for ($j=0;$j<56;$j++) $data[$index++]=($j<count($d))?ord($d[$j]):0; $d = str_split ( $conf ['nmea'] [$i] [1] );
// print_r($d);
// Print the result array as hex data for($j = 0; $j < 29; $j ++)
/* $data [$index ++] = ($j < count ( $d )) ? ord ( $d [$j] ) : 0;
print_r($conf); }
// echo "3\n";
echo "\n";
for ($i=0;$i<count($data);$i++) { // / Set default message
if (($i & 0xf)==0) printf ("\n%04x:",$i); $d = str_split ( $conf ['message'] );
printf (" %02x",$data[$i]); for($j = 0; $j < 56; $j ++)
} $data [$index ++] = ($j < count ( $d )) ? ord ( $d [$j] ) : 0;
echo "\n";
*/ // Print the result array as hex data
$bindata=""; /*
for($i=0;$i<count($data);$i++) $bindata.=chr($data[$i]); * print_r($conf);
$dev_imu_ctl = fopen('/dev/imu_ctl', 'w'); *
fwrite($dev_imu_ctl, $bindata, strlen($bindata)); * echo "\n";
fseek($dev_imu_ctl,3,SEEK_END); // start IMU * for ($i=0;$i<count($data);$i++) {
fclose($dev_imu_ctl); * if (($i & 0xf)==0) printf ("\n%04x:",$i);
/// Readback - just testing * printf (" %02x",$data[$i]);
$bd=file_get_contents('/dev/imu_ctl'); * }
$data =str_split($bd); * echo "\n";
if ($verbose) { */
echo "\n"; $bindata = "";
for ($i=0;$i<count($data);$i++) { for($i = 0; $i < count ( $data ); $i ++)
if (($i & 0xf)==0) printf ("\n%04x:",$i); $bindata .= chr ( $data [$i] );
printf (" %02x",ord($data[$i])); $dev_imu_ctl = fopen ( '/dev/imu_ctl', 'w' );
} fwrite ( $dev_imu_ctl, $bindata, strlen ( $bindata ) );
echo "\n"; fseek ( $dev_imu_ctl, 3, SEEK_END ); // start IMU
} fclose ( $dev_imu_ctl );
// / Readback - just testing
$bd = file_get_contents ( '/dev/imu_ctl' );
$data = str_split ( $bd );
if ($verbose) {
echo "\n";
for($i = 0; $i < count ( $data ); $i ++) {
if (($i & 0xf) == 0)
printf ( "\n%04x:", $i );
printf ( " %02x", ord ( $data [$i] ) );
}
echo "\n";
}
} }
......
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