def _updateAWA(self):
if STATIC_AWA == None:
# Sets the apparent wind angle
boat_bearing = self.arduinoData.hog
# Reverse direction for boat vector
if (boat_bearing >= 0):
boat_bearing -= 180
else:
boat_bearing += 180
boat_speed = self.arduinoData.sog
# Reverse direction for wind vector
wind_bearing = self.trueWindAngle
if (wind_bearing >= 0):
wind_bearing -= 180
else:
wind_bearing += 180
wind_speed = self.trueWindSpeed
boat_x = boat_speed * math.cos(boat_bearing)
boat_y = boat_speed * math.sin(boat_bearing)
wind_x = wind_speed * math.cos(wind_bearing)
wind_y = wind_speed * math.sin(wind_bearing)
x = boat_x + wind_x
y = boat_y + wind_y
if self.previousx is None:
self.previousx = x
awa = math.atan(y/x)
if (math.copysign(self.previousx, x) != self.previousx or self.flipflag):
if (not self.flipflag):
self.flipflag = True
elif (math.copysign(self.previousx, x) != self.previousx):
self.flipflag = False
print(str(self.previousx) + ", " + str(x))
if(awa > 0):
awa -= math.pi
else:
awa += math.pi
awa = math.degrees(awa)
awa -= self.arduinoData.hog
awa = standardcalc.boundTo180(awa)
self.previousx = x
else:
awa = standardcalc.boundTo180(standardcalc.boundTo360(STATIC_AWA)-standardcalc.boundTo180(self.arduinoData.hog))
self.arduinoData.awa = standardcalc.boundTo180(awa)
def interpretArr(self, ardArr):
arduinoData = datatypes.ArduinoData()
arduinoData.hog = standardcalc.boundTo180(ardArr[ARD_HOG])
arduinoData.cog = standardcalc.boundTo180(ardArr[ARD_COG])
arduinoData.sog = ardArr[ARD_SOG]
arduinoData.awa = ardArr[ARD_AWAV]
arduinoData.gps_coord = datatypes.GPSCoordinate(ardArr[ARD_LAT]/10000000, ardArr[ARD_LONG]/10000000)
arduinoData.sheet_percent = ardArr[ARD_SHT]
arduinoData.num_sat = ardArr[ARD_SAT]
arduinoData.gps_accuracy = ardArr[ARD_ACC]
arduinoData.auto = ardArr[ARD_AUT]
arduinoData.rudder = ardArr[ARD_RUD]
return arduinoData
def interpretArr(self, ardArr):
arduinoData = datatypes.ArduinoData()
arduinoData.hog = standardcalc.boundTo180(ardArr[ARD_HOG])
arduinoData.cog = standardcalc.boundTo180(ardArr[ARD_COG])
arduinoData.sog = ardArr[ARD_SOG]
arduinoData.awa = ardArr[ARD_AWAV]
arduinoData.gps_coord = datatypes.GPSCoordinate(
ardArr[ARD_LAT] / 10000000, ardArr[ARD_LONG] / 10000000)
arduinoData.sheet_percent = ardArr[ARD_SHT]
arduinoData.num_sat = ardArr[ARD_SAT]
arduinoData.gps_accuracy = ardArr[ARD_ACC]
arduinoData.auto = ardArr[ARD_AUT]
arduinoData.rudder = ardArr[ARD_RUD]
return arduinoData
def gybe(self, x):
tempspeed = self.arduinoData.sog
self.arduinoData.sog = 0
self.arduinoData.hog = standardcalc.boundTo180(self.arduinoData.hog+180)
gVars.logger.info("-------GYBE-----------")
time.sleep(4)
self.arduinoData.sog = tempspeed
def canLayMarkWithoutTack(self):
if standardcalc.isWPNoGoAWA(self.AWA, self.hog, self.Dest,self.sog,self.GPSCoord):
return False
else:
windDirection = standardcalc.boundTo180(self.AWA + self.hog)
bearing = standardcalc.angleBetweenTwoCoords(self.GPSCoord,self.Dest)
return not standardcalc.isAngleBetween(bearing,windDirection,self.hog)
def _updateTrueWind(self):
# Updates true wind angle
if (ALLOW_WIND_REVERSAL):
self.trueWindAngle += random.uniform(-.2, 0)
else:
self.trueWindAngle += random.uniform(-.1, .1)
self.trueWindAngle = standardcalc.boundTo180(self.trueWindAngle)
def _updateCOG(self):
# Sets the course over ground
if (math.fabs(self.arduinoData.cog + self.currplusmin - self.arduinoData.hog) < .4):
self.arduinoData.cog += round(random.uniform(-.1, .1), 2)
elif (self.arduinoData.cog + self.currplusmin < self.arduinoData.hog):
self.arduinoData.cog += round(random.uniform(0, 5), 2)
elif (self.arduinoData.cog + self.currplusmin > self.arduinoData.hog):
self.arduinoData.cog += round(random.uniform(-5, 0), 2)
self.arduinoData.cog = standardcalc.boundTo180(self.arduinoData.cog)
def canLayMarkWithoutTack(self):
if standardcalc.isWPNoGoAWA(self.AWA, self.hog, self.Dest, self.sog,
self.GPSCoord):
return False
else:
windDirection = standardcalc.boundTo180(self.AWA + self.hog)
bearing = standardcalc.angleBetweenTwoCoords(
self.GPSCoord, self.Dest)
return not standardcalc.isAngleBetween(bearing, windDirection,
self.hog)
def tack(self, weather, tack):
hog = self.arduinoData.hog
# Format
# Tack: Port=0 Stbd=1
if (tack == 1):
hog += 70
else:
hog -= 70
hog = standardcalc.boundTo180(hog)
gVars.logger.info("--------TACK----------")
self.arduinoData.hog = hog
self.arduinoData.awa = hog+STATIC_AWA
time.sleep(5)
def testGreaterThan180(self):
self.assertEqual(standardcalc.boundTo180(self.num2), self.num2bounded)
def testGreaterThan360ToNegative(self):
self.assertEqual(standardcalc.boundTo180(self.num5), self.num5bounded)
def testGreaterThan360ToPositive(self):
self.assertEqual(standardcalc.boundTo180(self.num4), self.num4bounded)
def testLessThan180(self):
self.assertEqual(standardcalc.boundTo180(self.num3), self.num3bounded)
def testGreaterThan180(self):
self.assertEqual(standardcalc.boundTo180(self.num2), self.num2bounded)
def testNoChange(self):
self.assertEqual(standardcalc.boundTo180(self.num1), self.num1bounded)
def roundBuoyPort(BuoyLoc, FinalBearing):
currentData = gVars.currentData
GPSCoord = currentData[gps_index]
# appWindAng = currentData[awa_index]
InitCog = currentData[cog_index] # Course over ground
InitHog = currentData[hog_index] # Heading over ground
X = 16.64 # Degrees, Calculated
Dest = 23.41 # Meters, Distance from boat to buoy
angleToNorth = standardcalc.angleBetweenTwoCoords(GPSCoord, BuoyLoc)
reflectLong = GPSCoord.long * -1 # Used for calculation ONLY, because longitude decreases from left to right
quadDir = None
if reflectLong > BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.sin(180 - angleToNorth + X)) * -1 # - X movement
moveLat = abs(math.cos(180 - angleToNorth + X)) * - 1 # - Y movement
quadDir = 3;
elif reflectLong < BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.cos(angleToNorth -90 - X)) # + X Movement
moveLat = abs(math.sin(angleToNorth - 90 - X)) * -1 # - Y Movement
quadDir = 4;
elif reflectLong < BuoyLoc.long and GPSCoord.lat < BuoyLoc.lat:
moveLong = abs(math.sin(angleToNorth - X)) # + X Movement
moveLat = abs(math.cos(angleToNorth - X)) # + Y Movement
quadDir = 1;
else:
moveLong = abs(math.sin(angleToNorth + X)) * - 1 # - X Movement
moveLat = abs(math.cos(angleToNorth + X)) # + Y Movement
quadDir = 2;
moveLong *= Dest
moveLat *= Dest
moveLong *= -1 # Convert back actual coordinates
destination = standardcalc.GPSDistAway(GPSCoord, moveLong, moveLat)
# 10 represents the degree of error around the destination point
# Calls point to point function until it reaches location past buoy
# Adding 10 does not increase the radius by 10 meters(ERROR!) - fixed
if (GPSCoord.long >= standardcalc.GPSDistAway(destination, 10, 0).long):# or GPSCoord.long <= standardcalc.GPSDistAway(destination, -10, 0).long) and (GPSCoord.lat >= standardcalc.GPSDistAway(destination, 0, 10).lat or GPSCoord.lat <= standardcalc.GPSDistAway(destination, 0, -10).lat):
pointToPoint(datatypes.GPSCoordinate(destination.lat, destination.long),1)
GPSCoord.long = gVars.currentData[gps_index].long
GPSCoord.lat = gVars.currentData[gps_index].lat
# Checks if the boat needs to round the buoy or just pass it
vect = datatypes.GPSCoordinate()
vect.lat = BuoyLoc.lat - currentData[gps_index].lat
vect.long = BuoyLoc.long - currentData[gps_index].long
# Checks if the boat as to round the buoy
buoyAngle = None
buoyAngle = standardcalc.vectorToDegrees(vect.lat, vect.long)
buoyAngle -= 90
buoyAngle = standardcalc.boundTo180(buoyAngle) #git later
# Incomplete, not static values, need to use trig to determine new gps locations
if FinalBearing < buoyAngle and FinalBearing > (buoyAngle - 90):
if reflectLong > BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong2 = abs(math.cos(180 - angleToNorth + X)) * -1 # - X movement
moveLat2 = abs(math.sin(180 - angleToNorth + X)) * - 1 # - Y movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong2 = abs(math.sin(angleToNorth -90 - X)) # + X Movement
moveLat2 = abs(math.cos(angleToNorth - 90 - X)) * -1 # - Y Movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat < BuoyLoc.lat:
moveLong2 = abs(math.cos(angleToNorth - X)) # + X Movement
moveLat2 = abs(math.sin(angleToNorth - X)) # + Y Movement
else:
moveLong2 = abs(math.sin(angleToNorth - X)) * - 1 # - X Movement
moveLat2 = abs(math.cos(angleToNorth - X)) # + Y Movement
destination = standardcalc.GPSDistAway(GPSCoord, moveLong2, moveLat2)
# 10 represents the degree of error around the destination point
# Calls point to point function until it reaches location past buoy
# Adding 10 does not increase the radius by 10 meters(ERROR!) - fixed
if (GPSCoord.long >= standardcalc.GPSDistAway(destination, 10, 0).long or GPSCoord.long <= standardcalc.GPSDistAway(destination, -10, 0).long) and (GPSCoord.lat >= standardcalc.GPSDistAway(destination, 0, 10).lat or GPSCoord.lat <= standardcalc.GPSDistAway(destination, 0, -10).lat):
pointToPoint(datatypes.GPSCoordinate(destination.lat, destination.long),1)
GPSCoord.long = gVars.currentData[gps_index].long
GPSCoord.lat = gVars.currentData[gps_index].lat
return 0
def testNoChange(self):
self.assertEqual(standardcalc.boundTo180(self.num1), self.num1bounded)
def testLessThan360ToNegative(self):
self.assertEqual(standardcalc.boundTo180(self.num6), self.num6bounded)
def testLessThan180(self):
self.assertEqual(standardcalc.boundTo180(self.num3), self.num3bounded)
def testLessThan360ToPositive(self):
self.assertEqual(standardcalc.boundTo180(self.num7), self.num7bounded)