start_gps_compass.php 24.8 KB
Newer Older
1
#!/usr/bin/env php
Andrey Filippov's avatar
Andrey Filippov committed
2 3
<?php
/*
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
 * ! FILE NAME : start_gps_compass.php
 * ! DESCRIPTION: Looks for USB GPS (currently Garmin GPS 18 USB) and compass
 * ! (currently Ocean Server OS-5000), re-initializes Exif header
 * ! and starts the devices if found
 * !
 * ! Copyright (C) 2008 Elphel, Inc
 * ! -----------------------------------------------------------------------------**
 * !
 * ! This program is free software: you can redistribute it and/or modify
 * ! it under the terms of the GNU General Public License as published by
 * ! the Free Software Foundation, either version 3 of the License, or
 * ! (at your option) any later version.
 * !
 * ! This program is distributed in the hope that it will be useful,
 * ! but WITHOUT ANY WARRANTY; without even the implied warranty of
 * ! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * ! GNU General Public License for more details.
 * !
 * ! You should have received a copy of the GNU General Public License
 * ! along with this program. If not, see <http://www.gnu.org/licenses/>.
 * ! -----------------------------------------------------------------------------**
 * ! $Log: start_gps_compass.php,v $
 * ! Revision 1.6 2012/03/14 00:05:41 elphel
 * ! 8.2.1 - GPS speed, x353.bit file tested on Eyesis
 * !
 * ! Revision 1.5 2011/08/13 00:55:14 elphel
 * ! support for detection 103595 and 103696 boards, programming the logger
 * !
 * ! Revision 1.4 2011/01/15 23:22:10 elphel
 * ! added 19200
 * !
 * ! Revision 1.3 2009/02/20 21:49:52 elphel
 * ! bugfix - was killing daemons after detection, not before (as should)
 * !
 * ! Revision 1.2 2009/02/19 22:38:37 elphel
 * ! 8.0.2.3 - added several USB serial adapters, nmea->exif support, detection of NMEA GPS at boot
 * !
 * ! Revision 1.1.1.1 2008/11/27 20:04:01 elphel
 * !
 * !
 * ! Revision 1.2 2008/11/20 07:03:40 elphel
 * ! exit value now encodes detected devices
 * !
 * ! Revision 1.1 2008/04/07 09:14:47 elphel
 * ! Discover/startup GPS and compass
 * !
 */
// look uptime, sleep if just started (no USB yet)
// require '/usr/html/includes/i2c.inc';
// require 'i2c.inc';
54
set_include_path ( get_include_path () . PATH_SEPARATOR . '/www/pages/include' );
Andrey Filippov's avatar
Andrey Filippov committed
55
require 'i2c_include.php';
56 57 58 59 60 61 62 63 64 65 66
$config_name = '/etc/elphel393/imu_logger.xml';
$gps_compass_xml = '/www/pages/gps_compass.xml';
$tmp_config = '/var/volatile/html/logger_config.xml';
$exif_php = "/www/pages/exif.php";
$bindir = '/usr/bin/';
$i2cbus = 4;
$ids = scanGrandDaughters ();
$b_index = indexGrandDaughters ( $ids );
// print_r($ids);
// print_r($b_index);
$baud = null;
67
$binfile = "/var/imu_config.bin"; // comment out if not needed - debug feature
68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
if (isset ( $b_index [103696] )) {
	$baud = $ids [$b_index [103696]] ['baud'];
	if ($baud === 0)
		$baud == 'auto';
}

$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";
Andrey Filippov's avatar
Andrey Filippov committed
153

154
$cmd = $exif_php . " init $noGPS $nocompass";
155 156 157 158 159 160 161
if ($verbose)
	echo "Initializing Exif template: $cmd\n";
exec ( $cmd );
if ($GPS) {
	if ($verbose)
		echo "Starting " . $GPS ["name"] . " as " . $GPS ["file"] . "\n";
	if (strpos ( $GPS ['name'], 'NMEA' ) !== false) {
162
		$cmd = $bindir . "nmea2exif " . $GPS ["file"] . " &";
163
	} else {
164
		$cmd = $bindir . "garminusb2exif " . $GPS ["file"] . " &"; // BUG: Not ported!
165 166 167 168 169 170 171 172 173
	}
	if ($verbose)
		echo "exec: $cmd \n";
	popen ( $cmd, "r" );
}
if ($compass) {
	if ($verbose)
		echo "Starting " . $compass ["name"] . " as " . $compass ["file"] . "\n";
	exec ( "stty -F " . $compass ["file"] . " -echo speed 19200" );
174
	$cmd = $bindir . "compass " . $compass ["file"] . " &";
175 176 177 178 179 180
	if ($verbose)
		echo "exec: $cmd \n";
	popen ( $cmd, "r" );
}
exit ( (isset ( $b_index [103695] ) ? 4 : 0) | ($compass ? 2 : 0) | ($GPS ? 1 : 0) );
function scanGrandDaughters() {
Andrey Filippov's avatar
Andrey Filippov committed
181
	global $i2cbus;
182 183 184 185 186 187 188 189 190 191 192 193 194
	$ids = array ();
	$id = "";
	for($i = 2; $i < 8; $i ++) {
		$id = i2c_read256b ( 0xa0 + 2 * $i, $i2cbus );
		$xml = simplexml_load_string ( $id );
		if (($xml !== false) && ($xml->getName () == 'board')) {
			$ids [$i] = array ();
			foreach ( $xml->children () as $entry ) {
				$ids [$i] [$entry->getName ()] = ( string ) $entry;
			}
		}
	}
	return $ids;
Andrey Filippov's avatar
Andrey Filippov committed
195 196
}
function indexGrandDaughters($ids) {
197 198 199 200 201 202
	$index = array ();
	foreach ( $ids as $i => $id )
		if (isset ( $id ['model'] )) {
			$index [$id ['model']] = $i;
		}
	return $index;
Andrey Filippov's avatar
Andrey Filippov committed
203
}
204
// / compass/GPS programs should be killed before detection
Andrey Filippov's avatar
Andrey Filippov committed
205 206 207
// baud=null - search USB Garmin and compass
// baud=auto - try to autodetect baud rate
// baud=number - used specified baud rate, do not wait for the GPS response
208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295
function find_gps_compass($baud = null, $timeout = 5) {
	global $verbose;
	$ttyOptions = array (
			'-echo speed 115200',
			'-echo speed 57600',
			'-echo speed 56000',
			'-echo speed 38400',
			'-echo speed 19200', // / maybe other options, like '-echo speed 4800 -ixoff'
			'-echo speed 9600',
			'-echo speed 4800',
			'-echo speed 2400' 
	);
	if (isset ( $baud ) && ($baud != 'auto') && ($baud != 0)) {
		$ttyOptions = array (
				'-echo speed ' . $baud 
		);
	}
	// global $GPS, $compass;
	$devices = array (
			"compass" => array (
					array (
							"name" => "Ocean Server OS-5000",
							"driver" => "cp2101" 
					) 
			),
			"GPS" => array (
					array (
							"name" => "Garmin GPS 18 USB",
							"driver" => "garmin_gps" 
					) 
			) 
	);
	exec ( "ls /sys/bus/usb-serial/devices", $usb_ser_devs ); // ("ttyUSB0","ttyUSB1")
	$devs = new SimpleXMLElement ( "<?xml version='1.0' standalone='yes'?><USB_serial_devices/>" );
	$serialDevices = array ();
	$compassPresent = false;
	$gpsPresent = false;
	$driver = array ();
	// echo "usb_ser_devs"; var_dump($usb_ser_devs);
	foreach ( $usb_ser_devs as $dev ) {
		// 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 (! isset ( $baud )) { // if baud is set (auto or number - skip compass, it is 103696)
			foreach ( $devices ["compass"] as $d ) {
				if ($d ["driver"] == $driver [$dev]) {
					$dd = $devs->addChild ( 'compass' );
					$dd->addChild ( 'name', $d ["name"] );
					$dd->addChild ( 'driver', $d ["driver"] );
					$dd->addChild ( 'file', "/dev/" . $dev );
					$serialDevices [$dev] = 'compass';
					$compassPresent = true;
				}
			}
		}
		if ($serialDevices [$dev])
			continue;
		foreach ( $devices ["GPS"] as $d ) {
			if ($d ["driver"] == $driver [$dev]) {
				$dd = $devs->addChild ( 'GPS' );
				$dd->addChild ( 'name', $d ["name"] );
				$dd->addChild ( 'driver', $d ["driver"] );
				$dd->addChild ( 'file', "/dev/" . $dev );
				$serialDevices [$dev] = 'gps';
				$gpsPresent = true;
			}
		}
	}
	// / Do we need to look for a NMEA GPS?
	if (! $gpsPresent)
		foreach ( $serialDevices as $dev => $type )
			if (! $type && ! $gpsPresent) {
				if ($verbose)
					printf ( "could be $dev\n" );
				foreach ( $ttyOptions as $ttyOpt ) {
					$cmd = 'stty -F /dev/' . $dev . ' ' . $ttyOpt;
					if (count ( $ttyOptions ) == 1) { // / single baud option, will not wait for confirmation. Has to be only one serial adapter !!
						if ($verbose)
							echo "Setting:  $cmd\n";
						exec ( $cmd );
						$gpsPresent = true;
					} else {
						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
296
						                                                                   // var_dump($fullOutput);
297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326
						foreach ( $fullOutput as $line )
							if (strpos ( $line, '$GP' ) === 0) {
								$gpsPresent = true;
								break;
							}
					}
					if ($gpsPresent) {
						$dd = $devs->addChild ( 'GPS' );
						$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;
Andrey Filippov's avatar
Andrey Filippov committed
327
}
328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354
function listSensorDevices($devs, $b_index, $ids) {
	global $GPS, $compass, $gps_compass_xml;
	
	foreach ( $devs->compass as $a ) {
		$compass = array (
				"file" => ( string ) $a->file,
				"name" => ( string ) $a->name 
		);
		break; // use first one
	}
	foreach ( $devs->GPS as $a ) {
		$GPS = array (
				"file" => ( string ) $a->file,
				"name" => ( string ) $a->name,
				"baud" => ( string ) $a->baud 
		);
		if (isset ( $b_index [103696] )) {
			$a->addChild ( 'part_number', $ids [$b_index [103696]] ['part'] );
			$a->addChild ( 'port', $ids [$b_index [103696]] ['port'] );
			$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 );
Andrey Filippov's avatar
Andrey Filippov committed
355
}
356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378
function loggerConfigToXML($conf) {
	$logger_xml = new SimpleXMLElement ( "<?xml version='1.0' standalone='yes'?><Logger_configuration/>" );
	foreach ( $conf as $key => $value ) {
		switch ($key) {
			case 'imu_registers' :
				$subtree = $logger_xml->addChild ( $key );
				for($i = 0; $i < count ( $value ); $i ++) {
					$subtree->addChild ( 'R_' . sprintf ( '%02d', $i ), sprintf ( "0x%02x", $value [$i] ) );
				}
				break;
			case 'nmea' :
				$subtree = $logger_xml->addChild ( $key );
				for($i = 0; $i < count ( $value ); $i ++) {
					$sentence = $subtree->addChild ( 'S_' . sprintf ( '%01d', $i ) );
					$sentence->addChild ( 'sentence', $value [$i] [0] );
					$sentence->addChild ( 'format', $value [$i] [1] );
				}
				break;
			default :
				$logger_xml->addChild ( $key, $value );
		}
	}
	return $logger_xml;
Andrey Filippov's avatar
Andrey Filippov committed
379
}
380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417
function loggerConfigFromXML($confXML) {
	$conf = array ();
	// var_dump($confXML);
	foreach ( $confXML as $item ) {
		// var_dump($item);
		$key = $item->getName ();
		switch ($key) {
			case 'imu_registers' :
				$conf [$key] = array ();
				foreach ( $item as $reg ) {
					$rname = substr ( $reg->getName (), 2 );
					$conf [$key] [$rname + 0] = (( string ) $reg) + 0;
				}
				break;
			case 'nmea' :
				$conf [$key] = array ();
				foreach ( $item as $reg ) {
					$rname = substr ( $reg->getName (), 2 );
					$conf [$key] [$rname + 0] = array (
							( string ) $reg->sentence,
							( string ) $reg->format 
					);
				}
				break;
			case 'imu_period' :
				$conf [$key] = ( string ) $item;
				if ($conf [$key] != 'auto')
					$conf [$key] += 0;
				break;
			case 'message' :
				$conf [$key] = ( string ) $item;
				break;
			default :
				// $conf[$key]=(int) $item;
				$conf [$key] = (( string ) $item) + 0;
		}
	}
	return $conf;
Andrey Filippov's avatar
Andrey Filippov committed
418
}
419 420 421 422 423 424 425 426 427
function init_default_config() {
	$default_config = array (
			'imu_period' => 'auto', // 0xFFFFFFFF, // 0 - off, >=0xff000000 - "when ready", else - number of SCLK periods
			'sclk_freq' => 5000000, // $clkr_divr= 8, // 80MHz divisor to get half SCLK rate (defaulkt SCLK=5MHz)
			'stall' => 2, // SPI stall time in usec
			'baud_rate' => 19200, // $rs232_div=80/2/$baud
			'imu_slot' => 1,
			'gps_slot' => 2,
			'gps_mode' => 2, // 0 - pps pos, 1 - pps neg, 2 - start of first sesntence after pause, 3 start of sentence
428 429
			'msg_conf' => 6, // 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
			'img_sync' => 0xf, // enable logging image acquisition starts (0 - disable), bit per channel
430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466
			'extra_conf' => 4, // 0 - config_long_sda_en, 1 -config_late_clk, 2 - config_single_wire, should be set for 103695 rev "A"
			'slow_spi' => 0, // set to 1 for slow SPI devices (0 for ADIS-16375)
			'imu_sa' => 3, // i2c slave address modifier for the 103695A pca9500
			'sleep_busy' => 30000, // microseconds
			'imu_registers' => array ( // / Up to 28 total
					0x10, // x gyro low
					0x12, // x gyro high
					0x14, // y gyro low
					0x16, // y gyro high
					0x18, // z gyro low
					0x1a, // z gyro high
					
					0x1c, // x accel low
					0x1e, // x accel high
					0x20, // y accel low
					0x22, // y accel high
					0x24, // z accel low
					0x26, // z accel high
					
					0x40, // x delta angle low
					0x42, // x delta angle high
					0x44, // y delta angle low
					0x46, // y delta angle high
					0x48, // z delta angle low
					0x4a, // z delta angle high
					
					0x4c, // x delta velocity low
					0x4e, // x delta velocity high
					0x50, // y delta velocity low
					0x52, // y delta velocity high
					0x54, // z delta velocity low
					0x56, // z delta velocity high
					
					0x0e, // temperature
					0x70, // time m/s
					0x72, // time d/h
					0x74 
467
			), // time y/m
468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488
			'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"
					// / "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
					array (
							'RMC',
							'nbnbnbnnnnb' 
					),
					array (
							'GGA',
							'nnbnbnnnnbnbbb' 
					),
					array (
							'GSA',
							'bnnnnnnnnnnnnnnnn' 
					),
					array (
							'VTG',
							'nbnbnbnb' 
					) 
			),
			'message' => 'Odometer message' 
489 490
	); // Message - up to 56 bytes

491
	return $default_config;
Andrey Filippov's avatar
Andrey Filippov committed
492 493
}
/**
494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513
 * Combines $conf and $dflt arrays - all driver parameters are overwritten aftre the call
 */
function combineLoggerConfigs($new_conf, $dflt = null) {
	global $default_config;
	// $xclk_freq=80000000; // 80 MHz in NC353
	$xclk_freq = 100000000; // 100 MHz in NC393
	$data = array ();
	$index = 0;
	if (! isset ( $dflt )) {
		$dflt = $default_config;
	}
	$conf = $dflt;
	if (isset ( $new_conf ))
		foreach ( $conf as $key => $oldvalue )
			if (isset ( $new_conf [$key] ))
				$conf [$key] = $new_conf [$key];
		// / fix insufficient data from default
	
	while ( count ( $conf ['imu_registers'] ) < 28 )
		$conf ['imu_registers'] [count ( $conf ['imu_registers'] )] = 0; // zero pad if less than 28 registers
514
			                                                                 // / Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults
515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534
	while ( count ( $conf ['nmea'] ) < 4 ) {
		for($i = 0; $i < 4; $i ++) {
			$new = true;
			for($j = 0; $j < count ( $conf ['nmea'] ); $j ++)
				if ($conf ['nmea'] [$j] [0] == $dflt ['nmea'] [$i] [0]) { // compare sentence code last 3 letters
					$new = false;
					break;
				}
			if ($new) {
				$conf ['nmea'] [count ( $conf ['nmea'] )] = $dflt ['nmea'] [$i];
				break;
			}
		}
	}
	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;
Andrey Filippov's avatar
Andrey Filippov committed
535
}
536
function setup_IMU_logger($conf) {
537
	global $verbose, $binfile;
538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579
	// $xclk_freq=80000000; // 80 MHz in NC353
	$xclk_freq = 100000000; // 100 MHz in NC393
	$data = array ();
	$index = 0;
	// / program period
	if (strtolower ( $conf ['imu_period'] ) == 'auto')
		$conf ['imu_period'] = 0xffffffff;
	else
		$conf ['imu_period'] += 0;
	$data [$index ++] = $conf ['imu_period'] & 0xff;
	$data [$index ++] = ($conf ['imu_period'] >> 8) & 0xff;
	$data [$index ++] = ($conf ['imu_period'] >> 16) & 0xff;
	$data [$index ++] = ($conf ['imu_period'] >> 24) & 0xff;
	// / program SCLK divisor and SPI stall time
	$sclk_div = round ( $xclk_freq / $conf ['sclk_freq'] / 2 );
	if ($sclk_div < 1)
		$sclk_div = 1;
	else if ($sclk_div > 255)
		$sclk_div = 255;
	$conf ['sclk_freq'] = $xclk_freq / $sclk_div / 2;
	$stall = (($conf ['stall'] * ($xclk_freq / $sclk_div)) / 1000000);
	if ($stall < 1)
		$stall = 1; // / does 0 work? need to verify
	else if ($stall > 255)
		$stall = 255;
	$conf ['stall'] = $stall * 1000000 / ($xclk_freq / $sclk_div);
	$data [$index ++] = $sclk_div;
	$data [$index ++] = $stall;
	$data [$index ++] = 0;
	$data [$index ++] = 0;
	// / Program rs232 baud rate
	$rs232_div = round ( $xclk_freq / 2 / $conf ['baud_rate'] );
	$conf ['baud_rate'] = $xclk_freq / 2 / $rs232_div;
	$data [$index ++] = $rs232_div & 0xff;
	$data [$index ++] = ($rs232_div >> 8) & 0xff;
	$data [$index ++] = ($rs232_div >> 16) & 0xff;
	$data [$index ++] = ($rs232_div >> 24) & 0xff;
	// / Program logger configuration (possible to modify only some fields - maybe support it here?)
	$logger_conf = 0;
	// / #define IMUCR__IMU_SLOT__BITNM 0 // slot, where 103695 (imu) board is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11)
	// / #define IMUCR__IMU_SLOT__WIDTH 2
	$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
580
	                                                         
581
	// / #define IMUCR__GPS_CONF__BITNM 3 // slot, where 103695 (imu) bnoard is connected: 0 - none, 1 - J9, 2 - J10, 3 - J11)
582 583 584 585 586
	                                                         // / #define IMUCR__GPS_CONF__WIDTH 4 // bits 0,1 - slot #, same as for IMU_SLOT, bits 2,3:
	                                                         // 0 - ext pulse, leading edge,
	                                                         // 1 - ext pulse, trailing edge
	                                                         // 2 - start of the first rs232 character after pause
	                                                         // 3 - start of the last "$" character (start of each NMEA sentence)
587 588 589 590 591 592 593 594 595
	$logger_conf |= ((($conf ['gps_slot'] & 0x3) | (($conf ['gps_mode'] & 0x3) << 2)) | 0x10) << 3;
	// / #define IMUCR__MSG_CONF__BITNM 8 // source of external pulses to log:
	// / #define IMUCR__MSG_CONF__WIDTH 5 // bits 0-3 - number of fpga GPIO input 0..11 (i.e. 0x0a - external optoisolated sync input (J15)
	// 0x0f - disable MSG module
	// bit 4 - invert polarity: 0 - timestamp leading edge, log at trailing edge, 1 - opposite
	// 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__SYN_CONF__BITNM 14 // logging frame time stamps (may be synchronized by another camera and have timestamp of that camera)
	// / #define IMUCR__SYN_CONF__WIDTH 1 // 0 - disable, 1 - enable
596 597 598 599 600 601
///	$logger_conf |= (($conf ['img_sync'] & 0x1) | 0x2) << 14;
	//NC393 - per channel
	$logger_conf |= (($conf ['img_sync'] & 0xf) | 0x10) << 14;
	
	
	// / #define  IMUCR__RST_CONF__BITNM 19   ///< reset module // was 16 in NC353
602 603
	// / #define IMUCR__RST_CONF__WIDTH 1 // 0 - enable, 1 -reset (needs resettimng DMA address in ETRAX also)
	
604
	// / #define  IMUCR__DBG_CONF__BITNM 21   ///< several axtra IMU configuration bits (was 18 for NC353)
605
	// / #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"
606
	$logger_conf |= (($conf ['extra_conf'] & 0xf) | 0x10) << 21; // was 18 in NC353;
607 608 609
	// / next bits used by the driver only, not the FPGA
	// / ((SLOW_SPI & 1)<<23) | \
	// / (DFLT_SLAVE_ADDR << 24))
610 611 612 613 614 615 616
	//#define  IMUCR__SLOW_SPI__BITNM 26   ///< just for the driver, not written to FPGA (was 23 for NC353)
	//#define  IMUCR__SLOW_SPI__WIDTH 1    ///< 0 - normal, 1 - slow SPI (programmed over i2c)
	$logger_conf |= ($conf ['slow_spi'] & 0x1) << 26; // was 23 for NC353
	// #define  IMUCR__I2C_SA3__BITNM 28    ///< Low 3 bits of the SA7 of the PCA9500 slave address
	// #define  IMUCR__I2C_SA3__WIDTH 3     ///< Should match jumpers
	$logger_conf |= ($conf ['imu_sa'] & 0x7) << 28; // was 24 for NC353
	
617
	$data [$index ++] = $logger_conf & 0xff;                // DWORD 0x03
618 619 620 621
	$data [$index ++] = ($logger_conf >> 8) & 0xff;
	$data [$index ++] = ($logger_conf >> 16) & 0xff;
	$data [$index ++] = ($logger_conf >> 24) & 0xff;
	// / Set time driver will go to sleep if the data is not ready yet (less than full sample in the buffer)
622
	$data [$index ++] = $conf ['sleep_busy'] & 0xff;        // DWORD 0x04 (software only)   
623 624 625 626 627 628 629
	$data [$index ++] = ($conf ['sleep_busy'] >> 8) & 0xff;
	$data [$index ++] = ($conf ['sleep_busy'] >> 16) & 0xff;
	$data [$index ++] = ($conf ['sleep_busy'] >> 24) & 0xff;
	// / Set IMU register addresses to logger
	// echo "1\n";
	for($i = 0; $i < 28; $i ++)
		$data [$index ++] = $conf ['imu_registers'] [$i]; // truncate to 28
630 631
			                                                  // / Configure NMEA sentences to log. If there are less than 4 - copy additional (unique) from defaults
			                                                  // echo "2\n";
632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666
	for($i = 0; $i < 4; $i ++) {
		$d = str_split ( $conf ['nmea'] [$i] [0] );
		// print_r($d);
		for($j = 0; $j < 3; $j ++)
			$data [$index ++] = ord ( $d [$j] );
		$d = str_split ( $conf ['nmea'] [$i] [1] );
		// print_r($d);
		for($j = 0; $j < 29; $j ++)
			$data [$index ++] = ($j < count ( $d )) ? ord ( $d [$j] ) : 0;
	}
	// echo "3\n";
	
	// / Set default message
	$d = str_split ( $conf ['message'] );
	for($j = 0; $j < 56; $j ++)
		$data [$index ++] = ($j < count ( $d )) ? ord ( $d [$j] ) : 0;
		
		// Print the result array as hex data
		/*
	 * print_r($conf);
	 *
	 * echo "\n";
	 * for ($i=0;$i<count($data);$i++) {
	 * if (($i & 0xf)==0) printf ("\n%04x:",$i);
	 * printf (" %02x",$data[$i]);
	 * }
	 * echo "\n";
	 */
	$bindata = "";
	for($i = 0; $i < count ( $data ); $i ++)
		$bindata .= chr ( $data [$i] );
	$dev_imu_ctl = fopen ( '/dev/imu_ctl', 'w' );
	fwrite ( $dev_imu_ctl, $bindata, strlen ( $bindata ) );
	fseek ( $dev_imu_ctl, 3, SEEK_END ); // start IMU
	fclose ( $dev_imu_ctl );
667 668 669 670 671 672 673
	
	if (isset($binfile)){
		$dev_imu_ctl = fopen ( $binfile, 'w' );
		fwrite ( $dev_imu_ctl, $bindata, strlen ( $bindata ) );
		fclose ( $dev_imu_ctl );
	}
	
674 675 676 677 678 679 680 681 682 683 684 685
	// / 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";
	}
Andrey Filippov's avatar
Andrey Filippov committed
686 687
}

688 689 690
// ls /sys/bus/usb-serial/devices/ttyUSB0/driver -l
// $xml->addChild ('error','No sync capable board detected, use "role=self" for the onboard timer');
// $sxml=$xml->asXML();
Andrey Filippov's avatar
Andrey Filippov committed
691
?>