# Tinley NMEA 2000 Transducer Change Over Switch emulator
# https://www.tinleyelectronics.com/products.htm

# Target hardware: Yacht Devices YDNB-07 NMEA 2000 gateway
# NMEA 2000 configuration: This version assumes the transducers are on CAN1, wind sensor on CAN2 and chartplotter on CAN2 interface

# software works by analyzing wind data from CAN2 interface
# if the wind speed goes over the threshold, the wind angle is checked
# if the wind angle stays in range [360° - deadband angle .. 180° + deadband angle] for more than specified time interval, switch to the starboard transducer
# if the wind angle stays in rande [0° + deadband angle .. 180° - deadband angle] for more than specified time interval, switch to the port transducer
# transducer is selected by the device address


# Copyright (c) 2018 Yacht Devices LTD.

# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:

# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.

# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# Disable forwarding for all mismatched PGNs
FW_CAN1_TO_CAN2=OFF
FW_CAN2_TO_CAN1=OFF

# initialize the state machine
init(){
	# ***** USER MODIFIABLE SECTION *****
	
	# B variable sets the angle deadband
	# 5 = angle in degrees.
	B = cast ( 174.5 * 5, UINT16)
	
	# C variable sets the wind speed threshold
	# 0.5 = wind speed in knots
	C = cast (51.44 * 0.5, UINT16)
	
	# transducers address
	P = cast (0x10, UINT8 ) # port
	S = cast (0x11, UINT8 ) # starboard
	
	# time interval: wind angle should be in the switching range at least, milliseconds
	F = cast (10000, UINT32)

	# ***** USER NON-MODIFIABLE SECTION *****

	# D variable selects the transducer
	# D = 0 -> port
	# D = 1 -> starboard
	D = cast (0, UINT8)
	
	# flags to enter time wait
	M = cast (0, UINT8)
	K = cast (0, UINT8)

	# new transducer number (0|1)
	Q = cast (0, UINT8)
	
	# angle match flags (1=match)
	G = cast (0, UINT8)
	H = cast (0, UINT8)
	
}


# get wind sensor data from CAN2 
# read 130306 from CAN2

match (CAN2,0x01FD0200, 0x01FFFF00 ){

	# get wind reference 
	R = get (DATA+5, UINT8) & 7
	
	# check if we got AWA (wind reference = 2)
	if (R==2){
		
		# clear angle match flags
		G = 0
		H = 0 
		
		# get wind angle (in 10^-4 radians)
		A = get (DATA+3, UINT16)
		
		# get wind speed (in 10^-2 m/s)
		V = get (DATA+1, UINT16)
		
		if (V >= C){
			# wind reference is AWA and wind speed is greater then the threshold
		
			# if the port transducer was active
			if (D == 0){
			
				# if the wind is between (180°+deadband) and (360°-deadband)
				if (A >= (31416 + B)){
					if (A <= 62831 - B ){
						
						# set angle match flag
						G = 1 
						
						# check if we need to wait 
						if (K == 0){
							L = timer()
							K = 1 
						}
						else{
							if (timediff(L) > F){
								# select starboard transducer
								Q = 1
								# clear time wait flags
								M = 0
								K = 0
							}
						}
						
					}
				}
			}
			
			# if the starboard transducer was active
			else{
			
				# if the wind is between (0°+deadband) and (180°-deadband)
				if (A <= (31416 - B)){
					if (A >= B){
					
						# set angle match flag
						H = 1 
						
						# check if we need to wait 
						if (M == 0){
							N = timer()
							M = 1 
						}
						else{
							if (timediff(N) > F){
								# select port transducer
								Q = 0
								# clear time wait flags
								K = 0
								M = 0
							}
						}

					}
				}
			}
			
			# set active transducer
			D = Q 
			
		}
	}

}

# process flags in separate thread
# we can not process them in previous match statement, as the maximum number of "ifs" per match is already used

heartbeat(10){

	# if angle is not in switching range, we do not need to enter time wait
	if (G == 0){
		# clear time wait flag
		K = 0
	}
		
	if (H == 0){
		M = 0
	}
}


# send 128267 Depth 

match (CAN1, 0x1F50B00, 0x1FFFF00 ){

	# get transducer address
	T = get (0, UINT8)
	
	# if port transducer was set active, send port transducer data
	if (D == 0){ 
		if (T == P)){
			set(0, UINT8, addr())
			send()
		}
	}

	# if starboard transducer was set active, send starboard transducer data
	if (D == 1){
		if (T == S)){
			set(0, UINT8, addr())
			send()
		}
	}
}

# send 128259 Speed, Water Referenced 

match (CAN1, 0x1F50300, 0x1FFFF00 ){

	# get transducer address
	T = get (0, UINT8)
	
	# if port transducer was set active, send port transducer data
	if (D == 0){ 
		if (T == P)){
			set(0, UINT8, addr())
			send()
		}
	}

	# if starboard transducer was set active, send starboard transducer data
	if (D == 1){
		if (T == S)){
			set(0, UINT8, addr())
			send()
		}
	}
}

# End of program