Overig μC snippets

Omschrijving

programmacode

initialisatie. De overige programmadelen staan hieronder. Deze programma's zijn bedoeld voor in een UAV, „Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com”

uav_part1.bas

'This is the UAV scheduler program
'Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com
 
' {$STAMP BS2p}
' {$PBASIC 2.5}
 
' Set this to the total number of waypoints
TotalWaypoints CON 3
 
' Waypoints (decimal format: degrees.minutes_as_fractional_degrees.
' Put the degrees in the first CON; the fractional degrees (decimal minutes) in the second.
' These are just placeholders. Insert your own waypoints and adjust the TotalWaypoints number accordingly
Waypoint1Lat  CON 47
Waypoint1LatMin CON 5906
Waypoint1Long CON  122
Waypoint1LongMin CON 1239
Waypoint2Lat  CON 47
Waypoint2LatMin CON 5913
Waypoint2Long CON 122
Waypoint2LongMin CON 1225
Waypoint3Lat  CON 47
Waypoint3LatMin CON 5906
Waypoint3Long CON 122
Waypoint3LongMin CON 1224
'Waypoint4Lat  CON 37
'Waypoint4LatMin CON 3853
'Waypoint4Long CON 122
'Waypoint4LongMin CON 2327
'Waypoint5Lat  CON 37
'Waypoint5LatMin CON 3853
'Waypoint5Long CON 122
'Waypoint5LongMin CON 2327
'Waypoint6Lat  CON 37
'Waypoint6LatMin CON 3853
'Waypoint6Long CON 122
'Waypoint6LongMin CON 2327
'Waypoint7Lat  CON 37
'Waypoint7LatMin CON 3853
'Waypoint7Long CON 122
'Waypoint7LongMin CON 2327
'Waypoint8Lat  CON 37
'Waypoint8LatMin CON 3853

Navigatie

uav_part2.bas

' This is the program for UAV navigation.
' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com
' Bug fixes by Wayne Garris

' {$STAMP BS2p}
' {$PBASIC 2.5}

' -----[ I/O Definitions ]-------------------------------------------------

gio             PIN     15     ' connects to GPS Module SIO pin
sdat PIN 14                     'serial io for servo
chan5in         PIN       13   'swich channel input
' -----[ Constants ]-------------------------------------------------------
bauds CON  1021
'RX5in           CON     5
Scale       CON     $0C0
ra CON 0
hipulse CON 1
lowpulse CON 0
ch4 CON 4
T4800           CON     500
Open            CON     $8000
Baud            CON     Open | T4800    ' Open mode to allow daisy chaining
'pin2            CON     9

' GPS Module Commands
GetLat          CON     $05
GetLong         CON     $06
GetHead         CON     $09
 
' -----[ Variables ]-------------------------------------------------------

TotalWaypoints VAR Nib: CurrentWayNum VAR Nib
TempLat VAR Byte: TempLatMin VAR Word: TempLong VAR Byte: TempLongMin VAR Word
CurrentWayLat VAR Byte: CurrentWayLatMin VAR Word: CurrentWayLong VAR Byte: CurrentWayLongMin VAR Word
DeltaLat VAR Word: DeltaLong VAR Word
 
workVal   VAR      Word     ' for numeric conversions

degrees   VAR      Byte     ' latitude/longitude degrees
minutes   VAR      Byte     ' latitude/longitude minutes
minutesD  VAR      Word     ' latitude/longitude decimal minutes
dir       VAR      Byte     ' direction (latitude: 0 = N, 1 = S, longitude: 0 = E, 1 = W)

heading   VAR      Word     ' heading in 0.1 degrees

' ------[ Main ]-------------

Main:
DO
' We'll use workVal as a temporary variable here to save variable space
  PULSIN chan5in,1,workVal
    workVal= (workVal */ scale)/2
    IF workVal > 750 THEN  RUN 0'1900 is a safe value for a mid-point (if the value is greater than 1900, then the
      'gear switch has been moved to the “manual” position).
  GET 68, CurrentWayNum
  GET 69, TotalWaypoints
  IF CurrentWayNum <= TotalWaypoints THEN GOSUB GetCurrentWay ELSE GOSUB GoHome
  GOSUB Get_Lat
  GOSUB Get_Long
  GOSUB Check_Proximity
  GOSUB Get_Heading
  GOSUB Get_Angle
  GOSUB Get_Rudder_Angle
  GOSUB Steer
 
  'RUN 3

LOOP
' -----[ Subroutines ]-----------------------------------------------------
Get_Angle:
  DeltaLong = -1 * DeltaLong 'reverse to conform with ArcTangent coordinate standards
  DeltaLat = DeltaLat ** 839'(128/9999)adjust them so the max values are +- 128, to conform with ArcTangent standards.
  DeltaLong = DeltaLong ** 839'=deltalong * (128 / 9999)
  workVal = DeltaLong ATN DeltaLat ' reuse workVal (to save variable memory) to hold binary radian result of ArcTangent
  DEBUG "angle", workVal,CR,11
  RETURN
 
' ----------------------------------------------------
Get_Rudder_Angle:
  heading = 256 - heading + 64 ' Convert GPS brad frame of reference to Basic Stamp frame of reference (reversed and rotated)
  workVal = workVal - heading ' Rotate them both so Heading is at zero
  IF workVal < 0 THEN workVal = workVal + 256 'Make all angles positive. Left is less than 128; right is larger than 128
  IF workVal < 128 THEN workVal = 128 - workVal ELSE workVal = 256 - workVal ' Adjust to reflect that 0 and 256 are maximum deflection
  ' for the servo; 128 is the neutral position.
  RETURN
 
' ----------------------------------------------------

Steer:
' We're going to reuse a lot of variables here; pay no attention to their names.
' Basically, we're turning the rudder by the value of workVal
 DEBUG "steer",DEC workval,CR,11
 workval = workval + 500
 SEROUT  sdat, bauds+$8000,["!SC",ch4,ra,workval.LOWBYTE,workval.HIGHBYTE,CR]
  RETURN
 
' ----------------------------------------------------
Check_Proximity:
  'we'll reuse CurrentWayNum here to save variable space
  CurrentWayNum = CurrentWayLat - TempLat
 
  'we can safely assume that we're never more than one degree away from the next waypoint, so we can reduce the degree delta into
  ' 1, -1 or 0 and collapse degrees and minutes into one word, with one degree = 10,000 and minutes 1-9,999
  IF CurrentWayNum = 1 THEN CurrentWayLatMin = CurrentWayLatMin + 10000
  IF CurrentWayNum = -1 THEN TempLatMin = TempLatMin + 10000
  DeltaLat = CurrentWayLatMin - TempLatMin
 
  'now do the same for longitude
  CurrentWayNum = CurrentWayLong - TempLong
  IF CurrentWayNum = 1 THEN CurrentWayLongMin = CurrentWayLongMin + 10000
  IF CurrentWayNum = -1 THEN TempLongMin = TempLongMin + 10000
  DeltaLong = CurrentWayLongMin - TempLongMin
 
  'set this for how close to the waypoints you want to get (these are set for one hundredth of a degree.)
  'If we're within range, go to the incrementing subroutine and switch to the next program
  IF ABS DeltaLat <= 100 AND ABS DeltaLong <= 100 THEN GOTO Next_Waypoint
  DEBUG "lat to waypoint",DEC deltalat,CR,11
  DEBUG "long to waypoint",DEC deltalong,CR,11
RETURN
 
' ----------------------------------------------------
Next_Waypoint:
'increment the waypoint counter and temprarily exit the navigation program
  CurrentWayNum = CurrentWayNum + 1
  PUT 68, CurrentWayNum
  DEBUG "waypoint #",DEC currentwaynum,CR,11
 ' RUN 3
  RETURN
' ----------------------------------------------------

GetCurrentWay:
  GET (5+(3*CurrentWayNum)), CurrentWayLat
  GET (6+(3*CurrentWayNum)), Word CurrentWayLatMin
  GET (8+(3*CurrentWayNum)), CurrentWayLong
  GET (9+(3*CurrentWayNum)), Word CurrentWayLongMin
  RETURN
 
' ----------------------------------------------------
GoHome:
  GET 0, CurrentWayLat
  GET 1, Word CurrentWayLatMin
  GET 3, CurrentWayLong
  GET 4, Word CurrentWayLongMin
  RETURN
 
' ----------------------------------------------------
Get_Lat:
  SEROUT gio, Baud, ["!GPS", GetLat]
  SERIN  gio, Baud, 3000, No_Response, [degrees, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir]
 
  ' convert to decimal format
  workVal = (minutes * 1000 / 6) + (minutesD / 60)
  TempLat = degrees
  TempLatMin = workVal
  DEBUG "lat", DEC degrees,"deg",DEC minutes,".",DEC minutesd,"min",CR,11
  RETURN
 
' ----------------------------------------------------
Get_Long:
  SEROUT gio, Baud, ["!GPS", GetLong]
  SERIN  gio, Baud, 3000, No_Response, [degrees, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir]
 
  ' convert to decimal format
  workVal = (minutes * 1000 / 6) + (minutesD / 60)
  TempLong = degrees
  TempLongMin = workVal
  DEBUG "long" ,DEC degrees,"deg",DEC minutes,".",DEC minutesd,"min",CR,11
  RETURN
 
' ----------------------------------------------------
Get_Heading:
  SEROUT gio, Baud, ["!GPS", GetHead]
  SERIN  gio, Baud, 3000, No_Response, [heading.HIGHBYTE, heading.LOWBYTE]
  DEBUG "heading" ,DEC heading ,CR ,11
  heading = (heading*100)/141 'convert to binary degrees (1 degree = 1.41 binary degrees)

  RETURN
 
' ----------------------------------------------------
No_Response:
  PAUSE 1000
  GOTO Main

UAV hoofdprogramma

uav_part3.bas

' This is the main UAV program.
' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com

' This first program just passes RX rudder and elevator commands through the Basic Stamp and watches for the gear
' switch to change state. When that changes this program transfers control to the initialization program for autonomous control.
' Based on demonstration code created by Dan DeGard (http://www.seattlerobotics.org/encoder/200304/AirmailStamp.htm)

'{$Stamp BS2p, initialize, navigation, altitude}''Initialization program is loaded in program slot 1; navigation in 2; altitude hold in 3
' {$PBASIC 2.5}
''pin 15 = gps io
''pin 14 = servo io
''pin 13 = chan5 ,switch
''pin 12 = chan4 ,rudder
''pin 11 = chan3 ,throttle
''pin 10 = chan2 ,elevator
''pin 9 = chan1 , aileron

 
' --------[Variables

'permission VAR Word: CntrlInput VAR Word: Position VAR Word
'Lbyte VAR Byte: Ubyte VAR Byte: interim VAR Word
'RX5in CON 5: RX4in CON 7: RX2in CON 6

'These constants are used by the FT639

'pin1 CON 8: pin2 CON 9: baud CON 17405: header CON 103
'pulse CON 90 :

chan2in PIN 10 'channel 1 input
chan3in PIN 11
chan4in PIN 12 'channel 2 input
chan5in PIN 13   'swich channel input
sdat PIN 14
bauds CON  1021       'baud rate
ra CON 0
hipulse CON 1
lowpulse CON 0
ch2 CON 2
ch4 CON 4
 Scale       CON     $0C0
chan2 VAR Word   'channel 1 value
chan4 VAR Word   ' channel 2 value
chan5 VAR Word   ' swich value
'GOSUB setup 'these steps initialize the FT639.
'GOSUB setpulse '
'GOSUB setheader '
'GOSUB setactive '

'The following subroutine first checks to see if the “gear” switch has been changed. Then, it reads the length of the
' square-wave pulse from the R/C receiver. The time value of the pulse is converted arithmetically to a number
' between 0 and 128. This position number is then split into two 4-Bit nibbles and sent to the FT639 chip (all this
' is just to create the required syntax for the FT chip). Note: it's possible to generate a square wave and drive the servos
' with the Basic Stamp chip directly, but it's a computational drain and uses more scarce memory. We offload that
' to the FT639 chip to simplify matters.

check:
 
  PULSIN chan5in,1,chan5
  chan5 = (chan5 */ scale)/2
 
  DEBUG DEC chan5,TAB
 
  IF chan5 > 750  THEN GOSUB through_put ELSE GOSUB Auto
  'IF Permission < 1900 THEN Autonomous '1900 is a safe value for a mid-point (if the value is less than 1900, then the
    'gear switch has been moved to the “autonomous” position).
 'throughput sub
 through_put:
 
 PULSIN chan2in , HIpulse , chan2
 chan2 = (chan2 */ scale)/2
 DEBUG DEC chan2 ,TAB
 SEROUT  sdat, bauds+$8000,["!SC",ch2,ra,chan2.LOWBYTE,chan2.HIGHBYTE,CR]
 
 
 PULSIN chan4in , HIpulse , chan4
 chan4 = (chan4 */ scale)/2
 DEBUG DEC chan4 ,CR,11
 SEROUT  sdat, bauds+$8000,["!SC",ch4,ra,chan4.LOWBYTE,chan4.HIGHBYTE,CR]
 
 
 
 
 
GOTO check
 
Auto:
 
  DEBUG "auto on",CR,11
  RUN 1
END

hoogteregeling

uav_part4.bas

' This is the UAV altitude hold program
' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com

' {$STAMP BS2p}
' {$PBASIC 2.5}

' -----[ I/O Definitions ]-------------------------------------------------

Sio             PIN     15     ' connects to GPS Module SIO pin

 
' -----[ Constants ]-------------------------------------------------------

RX5in           CON     5
T4800           CON     500
Open            CON     $8000
Baud            CON     Open | T4800    ' Open mode to allow daisy chaining
GetAlt          CON     $07
pin2            CON     9
 
' -----[ Variables ] ------------------------------------------------------

Alt       VAR      Word(3)
AltOriginal VAR Word: Position VAR Word
Lbyte VAR Byte: Ubyte VAR Byte: interim VAR Word
Temp VAR Word
 
'------[ Main ]------------------------

Main:
  PULSIN RX5in,1,Temp
  IF Temp > 1900 THEN RUN 0 '1900 is a safe value for a mid-point (if the value is greater than 1900, then the
    'gear switch has been moved to the “manual” position).

    ' ------- Get three readings. First...-------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE, alt.LOWBYTE]
' ------- Second... -------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE(1), alt.LOWBYTE(1)]
' ------- Third...-------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE(2), alt.LOWBYTE(2)]
' ------- Average them ----------------
  alt = (alt + alt(1) + alt(2)) / 3
 
  ' move elevator as required to stay within +-10 meters of original altitude
  GET 6, Word AltOriginal
  IF alt < (AltOriginal + 10) OR alt > (AltOriginal - 10) THEN GOSUB Center
  IF alt >= (AltOriginal + 10) THEN GOSUB Down
  IF alt <= (AltOriginal - 10) THEN GOSUB Up
  DEBUG "Going to program 2"
  RUN 2
 
' --------[ Subroutines]--------
Center:
  Position = 128 'adjust this so it's centered
  Interim = Position & %00001111 'keep last 4 bits
  Lbyte = Interim | %00100000 'create lower nibble for servo 3 (0010 means “lower nibble for servo 3” in FT639-speak
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000 'create upper nibble for servo 3 (1010 means “upper nibble for servo 3” in FT639-speak
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10 'sends command to FT639
  RETURN
 
Down:
  Position = 100 'adjust this so it's a bit down
  Interim = Position & %00001111
  Lbyte = Interim | %00100000
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10
  RETURN
 
Up:
  Position = 150 'adjust this so it's a bit up
  Interim = Position & %00001111
  Lbyte = Interim | %00100000
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10
  RETURN
 
No_Response:
  PAUSE 5000
  GOTO Main

Rhumbline Distance/bearing. Friendly stolen from Chris Veness

These formulæ give the distance and (constant) bearing between two points.

  Formula: 	
                Δφ = ln(tan(lat2/2+π/4)/tan(lat1/2+π/4)) 	[= the ‘stretched’ latitude difference]
  if E:W line, 	q = cos(lat1) 	 
  otherwise, 	q = Δlat/Δφ 	 
	        d = √(Δlat² + q².Δlon²).R 	[pythagoras]
	        θ = atan2(Δlon, Δφ) 	 
	        where ln is natural log, Δlon is taking shortest route (<180º), and R is the earth’s radius

rdb.txt

var dPhi = Math.log(Math.tan(lat2/2+Math.PI/4)/Math.tan(lat1/2+Math.PI/4));
var q = (!isNaN(dLat/dPhi)) ? dLat/dPhi : Math.cos(lat1);  // E-W line gives dPhi=0
 
// if dLon over 180° take shorter rhumb across 180° meridian:
if (Math.abs(dLon) > Math.PI) {
  dLon = dLon>0 ? -(2*Math.PI-dLon) : (2*Math.PI+dLon);
}
var d = Math.sqrt(dLat*dLat + q*q*dLon*dLon) * R;
var brng = Math.atan2(dLon, dPhi);

Friendly stolen from Chris Veness

He will accept donations

sphere.txt

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
/*  Latitude/longitude spherical geodesy formulae & scripts (c) Chris Veness 2002-2010            */
/*   - www.movable-type.co.uk/scripts/latlong.html                                                */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
 
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
/*  Note that minimal error checking is performed in this example code!                           */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
 
 
/**
 * Creates a point on the earth's surface at the supplied latitude / longitude
 *
 * @constructor
 * @param {Number} lat: latitude in numeric degrees
 * @param {Number} lon: longitude in numeric degrees
 * @param {Number} [rad=6371]: radius of earth if different value is required from standard 6,371km
 */
function LatLon(lat, lon, rad) {
  if (typeof rad == 'undefined') rad = 6371;  // earth's mean radius in km
  this._lat = lat;
  this._lon = lon;
  this._radius = rad;
}
 
 
/**
 * Returns the distance from this point to the supplied point, in km 
 * (using Haversine formula)
 *
 * from: Haversine formula - R. W. Sinnott, "Virtues of the Haversine",
 *       Sky and Telescope, vol 68, no 2, 1984
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @param   {Number} [precision=4]: no of significant digits to use for returned value
 * @returns {Number} Distance in km between this point and destination point
 */
LatLon.prototype.distanceTo = function(point, precision) {
  // default 4 sig figs reflects typical 0.3% accuracy of spherical model
  if (typeof precision == 'undefined') precision = 4;  
 
  var R = this._radius;
  var lat1 = this._lat.toRad(), lon1 = this._lon.toRad();
  var lat2 = point._lat.toRad(), lon2 = point._lon.toRad();
  var dLat = lat2 - lat1;
  var dLon = lon2 - lon1;
 
  var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
          Math.cos(lat1) * Math.cos(lat2) * 
          Math.sin(dLon/2) * Math.sin(dLon/2);
  var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
  var d = R * c;
  return d.toPrecisionFixed(precision);
}
 
 
/**
 * Returns the (initial) bearing from this point to the supplied point, in degrees
 *   see http://williams.best.vwh.net/avform.htm#Crs
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Initial bearing in degrees from North
 */
LatLon.prototype.bearingTo = function(point) {
  var lat1 = this._lat.toRad(), lat2 = point._lat.toRad();
  var dLon = (point._lon-this._lon).toRad();
 
  var y = Math.sin(dLon) * Math.cos(lat2);
  var x = Math.cos(lat1)*Math.sin(lat2) -
          Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
  var brng = Math.atan2(y, x);
 
  return (brng.toDeg()+360) % 360;
}
 
 
/**
 * Returns final bearing arriving at supplied destination point from this point; the final bearing 
 * will differ from the initial bearing by varying degrees according to distance and latitude
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Final bearing in degrees from North
 */
LatLon.prototype.finalBearingTo = function(point) {
  // get initial bearing from supplied point back to this point...
  var lat1 = point._lat.toRad(), lat2 = this._lat.toRad();
  var dLon = (this._lon-point._lon).toRad();
 
  var y = Math.sin(dLon) * Math.cos(lat2);
  var x = Math.cos(lat1)*Math.sin(lat2) -
          Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
  var brng = Math.atan2(y, x);
 
  // ... & reverse it by adding 180°
  return (brng.toDeg()+180) % 360;
}
 
 
/**
 * Returns the midpoint between this point and the supplied point.
 *   see http://mathforum.org/library/drmath/view/51822.html for derivation
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {LatLon} Midpoint between this point and the supplied point
 */
LatLon.prototype.midpointTo = function(point) {
  lat1 = this._lat.toRad(), lon1 = this._lon.toRad();
  lat2 = point._lat.toRad();
  var dLon = (point._lon-this._lon).toRad();
 
  var Bx = Math.cos(lat2) * Math.cos(dLon);
  var By = Math.cos(lat2) * Math.sin(dLon);
 
  lat3 = Math.atan2(Math.sin(lat1)+Math.sin(lat2),
                    Math.sqrt( (Math.cos(lat1)+Bx)*(Math.cos(lat1)+Bx) + By*By) );
  lon3 = lon1 + Math.atan2(By, Math.cos(lat1) + Bx);
 
  return new LatLon(lat3.toDeg(), lon3.toDeg());
}
 
 
/**
 * Returns the destination point from this point having travelled the given distance (in km) on the 
 * given initial bearing (bearing may vary before destination is reached)
 *
 *   see http://williams.best.vwh.net/avform.htm#LL
 *
 * @param   {Number} brng: Initial bearing in degrees
 * @param   {Number} dist: Distance in km
 * @returns {LatLon} Destination point
 */
LatLon.prototype.destinationPoint = function(brng, dist) {
  dist = dist/this._radius;  // convert dist to angular distance in radians
  brng = brng.toRad();  // 
  var lat1 = this._lat.toRad(), lon1 = this._lon.toRad();
 
  var lat2 = Math.asin( Math.sin(lat1)*Math.cos(dist) + 
                        Math.cos(lat1)*Math.sin(dist)*Math.cos(brng) );
  var lon2 = lon1 + Math.atan2(Math.sin(brng)*Math.sin(dist)*Math.cos(lat1), 
                               Math.cos(dist)-Math.sin(lat1)*Math.sin(lat2));
  lon2 = (lon2+3*Math.PI)%(2*Math.PI) - Math.PI;  // normalise to -180...+180
 
  if (isNaN(lat2) || isNaN(lon2)) return null;
  return new LatLon(lat2.toDeg(), lon2.toDeg());
}
 
 
/**
 * Returns the point of intersection of two paths defined by point and bearing
 *
 *   see http://williams.best.vwh.net/avform.htm#Intersection
 *
 * @param   {LatLon} p1: First point
 * @param   {Number} brng1: Initial bearing from first point
 * @param   {LatLon} p2: Second point
 * @param   {Number} brng2: Initial bearing from second point
 * @returns {LatLon} Destination point (null if no unique intersection defined)
 */
LatLon.intersection = function(p1, brng1, p2, brng2) {
  lat1 = p1._lat.toRad(), lon1 = p1._lon.toRad();
  lat2 = p2._lat.toRad(), lon2 = p2._lon.toRad();
  brng13 = brng1.toRad(), brng23 = brng2.toRad();
  dLat = lat2-lat1, dLon = lon2-lon1;
 
  dist12 = 2*Math.asin( Math.sqrt( Math.sin(dLat/2)*Math.sin(dLat/2) + 
    Math.cos(lat1)*Math.cos(lat2)*Math.sin(dLon/2)*Math.sin(dLon/2) ) );
  if (dist12 == 0) return null;
 
  // initial/final bearings between points
  brngA = Math.acos( ( Math.sin(lat2) - Math.sin(lat1)*Math.cos(dist12) ) / 
    ( Math.sin(dist12)*Math.cos(lat1) ) );
  if (isNaN(brngA)) brngA = 0;  // protect against rounding
  brngB = Math.acos( ( Math.sin(lat1) - Math.sin(lat2)*Math.cos(dist12) ) / 
    ( Math.sin(dist12)*Math.cos(lat2) ) );
 
  if (Math.sin(lon2-lon1) > 0) {
    brng12 = brngA;
    brng21 = 2*Math.PI - brngB;
  } else {
    brng12 = 2*Math.PI - brngA;
    brng21 = brngB;
  }
 
  alpha1 = (brng13 - brng12 + Math.PI) % (2*Math.PI) - Math.PI;  // angle 2-1-3
  alpha2 = (brng21 - brng23 + Math.PI) % (2*Math.PI) - Math.PI;  // angle 1-2-3
 
  if (Math.sin(alpha1)==0 && Math.sin(alpha2)==0) return null;  // infinite intersections
  if (Math.sin(alpha1)*Math.sin(alpha2) < 0) return null;       // ambiguous intersection
 
  //alpha1 = Math.abs(alpha1);
  //alpha2 = Math.abs(alpha2);
  // ... Ed Williams takes abs of alpha1/alpha2, but seems to break calculation?
 
  alpha3 = Math.acos( -Math.cos(alpha1)*Math.cos(alpha2) + 
                       Math.sin(alpha1)*Math.sin(alpha2)*Math.cos(dist12) );
  dist13 = Math.atan2( Math.sin(dist12)*Math.sin(alpha1)*Math.sin(alpha2), 
                       Math.cos(alpha2)+Math.cos(alpha1)*Math.cos(alpha3) )
  lat3 = Math.asin( Math.sin(lat1)*Math.cos(dist13) + 
                    Math.cos(lat1)*Math.sin(dist13)*Math.cos(brng13) );
  dLon13 = Math.atan2( Math.sin(brng13)*Math.sin(dist13)*Math.cos(lat1), 
                       Math.cos(dist13)-Math.sin(lat1)*Math.sin(lat3) );
  lon3 = lon1+dLon13;
  lon3 = (lon3+Math.PI) % (2*Math.PI) - Math.PI;  // normalise to -180..180º
 
  return new LatLon(lat3.toDeg(), lon3.toDeg());
}
 
 
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
 
/**
 * Returns the distance from this point to the supplied point, in km, travelling along a rhumb line
 *
 *   see http://williams.best.vwh.net/avform.htm#Rhumb
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Distance in km between this point and destination point
 */
LatLon.prototype.rhumbDistanceTo = function(point) {
  var R = this._radius;
  var lat1 = this._lat.toRad(), lat2 = point._lat.toRad();
  var dLat = (point._lat-this._lat).toRad();
  var dLon = Math.abs(point._lon-this._lon).toRad();
 
  var dPhi = Math.log(Math.tan(lat2/2+Math.PI/4)/Math.tan(lat1/2+Math.PI/4));
  var q = (!isNaN(dLat/dPhi)) ? dLat/dPhi : Math.cos(lat1);  // E-W line gives dPhi=0
  // if dLon over 180° take shorter rhumb across 180° meridian:
  if (dLon > Math.PI) dLon = 2*Math.PI - dLon;
  var dist = Math.sqrt(dLat*dLat + q*q*dLon*dLon) * R; 
 
  return dist.toPrecisionFixed(4);  // 4 sig figs reflects typical 0.3% accuracy of spherical model
}
 
/**
 * Returns the bearing from this point to the supplied point along a rhumb line, in degrees
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Bearing in degrees from North
 */
LatLon.prototype.rhumbBearingTo = function(point) {
  var lat1 = this._lat.toRad(), lat2 = point._lat.toRad();
  var dLon = (point._lon-this._lon).toRad();
 
  var dPhi = Math.log(Math.tan(lat2/2+Math.PI/4)/Math.tan(lat1/2+Math.PI/4));
  if (Math.abs(dLon) > Math.PI) dLon = dLon>0 ? -(2*Math.PI-dLon) : (2*Math.PI+dLon);
  var brng = Math.atan2(dLon, dPhi);
 
  return (brng.toDeg()+360) % 360;
}
 
/**
 * Returns the destination point from this point having travelled the given distance (in km) on the 
 * given bearing along a rhumb line
 *
 * @param   {Number} brng: Bearing in degrees from North
 * @param   {Number} dist: Distance in km
 * @returns {LatLon} Destination point
 */
LatLon.prototype.rhumbDestinationPoint = function(brng, dist) {
  var R = this._radius;
  var d = parseFloat(dist)/R;  // d = angular distance covered on earth's surface
  var lat1 = this._lat.toRad(), lon1 = this._lon.toRad();
  brng = brng.toRad();
 
  var lat2 = lat1 + d*Math.cos(brng);
  var dLat = lat2-lat1;
  var dPhi = Math.log(Math.tan(lat2/2+Math.PI/4)/Math.tan(lat1/2+Math.PI/4));
  var q = (!isNaN(dLat/dPhi)) ? dLat/dPhi : Math.cos(lat1);  // E-W line gives dPhi=0
  var dLon = d*Math.sin(brng)/q;
  // check for some daft bugger going past the pole
  if (Math.abs(lat2) > Math.PI/2) lat2 = lat2>0 ? Math.PI-lat2 : -(Math.PI-lat2);
  lon2 = (lon1+dLon+3*Math.PI)%(2*Math.PI) - Math.PI;
 
  return new LatLon(lat2.toDeg(), lon2.toDeg());
}
 
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
 
 
/**
 * Returns the latitude of this point; signed numeric degrees if no format, otherwise format & dp 
 * as per Geo.toLat()
 *
 * @param   {String} [format]: Return value as 'd', 'dm', 'dms'
 * @param   {Number} [dp=0|2|4]: No of decimal places to display
 * @returns {Number|String} Numeric degrees if no format specified, otherwise deg/min/sec
 *
 * @requires Geo
 */
LatLon.prototype.lat = function(format, dp) {
  if (typeof format == 'undefined') return this._lat;
 
  return Geo.toLat(this._lat, format, dp);
}
 
/**
 * Returns the longitude of this point; signed numeric degrees if no format, otherwise format & dp 
 * as per Geo.toLon()
 *
 * @param   {String} [format]: Return value as 'd', 'dm', 'dms'
 * @param   {Number} [dp=0|2|4]: No of decimal places to display
 * @returns {Number|String} Numeric degrees if no format specified, otherwise deg/min/sec
 *
 * @requires Geo
 */
LatLon.prototype.lon = function(format, dp) {
  if (typeof format == 'undefined') return this._lon;
 
  return Geo.toLon(this._lon, format, dp);
}
 
/**
 * Returns a string representation of this point; format and dp as per lat()/lon()
 *
 * @param   {String} [format]: Return value as 'd', 'dm', 'dms'
 * @param   {Number} [dp=0|2|4]: No of decimal places to display
 * @returns {String} Comma-separated latitude/longitude
 *
 * @requires Geo
 */
LatLon.prototype.toString = function(format, dp) {
  if (typeof format == 'undefined') format = 'dms';
 
  return Geo.toLat(this._lat, format, dp) + ', ' + Geo.toLon(this._lon, format, dp);
}
 
 
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
 
// extend Number object with methods for converting degrees/radians
 
/** Convert numeric degrees to radians */
if (typeof(String.prototype.toRad) === "undefined") {
  Number.prototype.toRad = function() {
    return this * Math.PI / 180;
  }
}
 
/** Convert radians to numeric (signed) degrees */
if (typeof(String.prototype.toDeg) === "undefined") {
  Number.prototype.toDeg = function() {
    return this * 180 / Math.PI;
  }
}
 
/** 
 * Format the significant digits of a number, using only fixed-point notation (no exponential)
 * 
 * @param   {Number} precision: Number of significant digits to appear in the returned string
 * @returns {String} A string representation of number which contains precision significant digits
 */
if (typeof(String.prototype.toDeg) === "undefined") {
  Number.prototype.toPrecisionFixed = function(precision) {
    var numb = this < 0 ? -this : this;  // can't take log of -ve number...
    var sign = this < 0 ? '-' : '';
 
    if (numb == 0) { n = '0.'; while (precision--) n += '0'; return n };  // can't take log of zero
 
    var scale = Math.ceil(Math.log(numb)*Math.LOG10E);  // no of digits before decimal
    var n = String(Math.round(numb * Math.pow(10, precision-scale)));
    if (scale > 0) {  // add trailing zeros & insert decimal as required
      l = scale - n.length;
      while (l-- > 0) n = n + '0';
      if (scale < n.length) n = n.slice(0,scale) + '.' + n.slice(scale);
    } else {          // prefix decimal and leading zeros if required
      while (scale++ < 0) n = '0' + n;
      n = '0.' + n;
    }
    return sign + n;
  }
}
 
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */

Omschrijving

programmacode

Omschrijving

programmacode