1 /***************************************************************************
2 towedarray.cpp - description
3 -------------------
4 begin : Sat Mar 16 2002
5 copyright : (C) 2002 by Michael Bridak
6 email : michael.bridak@verizon.net
7 $Id: towedarray.cpp,v 1.3 2003/04/14 05:51:04 mbridak Exp $
8 ***************************************************************************/
9
10 /***************************************************************************
11 * *
12 * This program is free software; you can redistribute it and/or modify *
13 * it under the terms of the GNU General Public License as published by *
14 * the Free Software Foundation; either version 2 of the License. *
15 * . *
16 * *
17 ***************************************************************************/
18
19 #include "towedarray.h"
20 #include "math.h"
21
TowedArray()22 TowedArray::TowedArray(){
23 length = 0;
24 cut = FALSE;
25 }
TowedArray(double latitude,double longitude)26 TowedArray::TowedArray(double latitude, double longitude){
27 for (int counter=0; counter<100; counter--){
28 latitude_array[counter] = latitude;
29 longitude_array[counter] = longitude;
30 }
31 length = 0;
32 cut = FALSE;
33 }
~TowedArray()34 TowedArray::~TowedArray(){
35 }
RecordPos(double latitude,double longitude)36 void TowedArray::RecordPos(double latitude, double longitude){
37 for (int counter=99; counter>0; counter--){
38 latitude_array[counter] = latitude_array[counter-1];
39 longitude_array[counter] = longitude_array[counter-1];
40 }
41 latitude_array[0] = latitude;
42 longitude_array[0] = longitude;
43 }
Extend()44 int TowedArray::Extend(){
45 if (! cut)
46 {
47 winch = 1;
48 return TRUE;
49 }
50 return FALSE;
51 }
Stop()52 void TowedArray::Stop(){
53 winch = 0;
54 }
ReelIn()55 int TowedArray::ReelIn(){
56 if (! cut)
57 {
58 winch = 2;
59 return TRUE;
60 }
61 return FALSE;
62 }
CutArray()63 int TowedArray::CutArray()
64 {
65 // if the array is already cut or reeled in, we can't cut it
66 if (cut)
67 return FALSE;
68 if (length == 0)
69 return FALSE;
70 cut = TRUE;
71 length = 0;
72 winch = 0;
73 return TRUE;
74 }
GetLength()75 int TowedArray::GetLength(){
76 return (int)length;
77 }
OperateWinch()78 void TowedArray::OperateWinch(){
79 switch(winch){
80 case 0: //Winch is stopped, Do nothing.
81 break;
82 case 1: //Extend the winch.
83 if(length<2600){
84 length += 5; //5ft per second @ 1/5th sec interval
85 }
86 break;
87 case 2: //Retract the winch.
88 if(length>0){
89 length -= 5;
90 }
91 break;
92 default: //just in case.....
93 //cerr << "Uh, Capt'n somethings wrong with the winch." << endl;
94 break;
95 }
96 }
97
ReturnLatLon(double & rlat,double & rlon)98 void TowedArray::ReturnLatLon(double &rlat, double &rlon){
99 int index;
100 index = (int)((length/3)/10);
101 rlat = latitude_array[index];
102 rlon = longitude_array[index];
103 }
104
ReturnHeading()105 float TowedArray::ReturnHeading(){
106 int head_index;
107 int tail_index;
108 float bearing; //atan() needs doubles
109 bearing =-1; //if the array is not extended enough
110 //bearing will return as a neg #
111 if(length > 240){
112 tail_index = (int)((length/3)/10);
113 head_index = tail_index - 8;
114 bearing = CalcBearing(latitude_array[tail_index], longitude_array[tail_index], latitude_array[head_index], longitude_array[head_index]);
115 }
116 return bearing;
117 }
118
BearingAmbiguity(float bearing)119 float TowedArray::BearingAmbiguity(float bearing){
120
121 float ambiguous_bearing;
122 float our_bearing;
123 float bearing_difference;
124 our_bearing=ReturnHeading();
125 bearing_difference = our_bearing - bearing;
126 if(bearing_difference < 0) bearing_difference += 360;
127
128 ambiguous_bearing = our_bearing + bearing_difference;
129 return ambiguous_bearing;
130 }
131
BearingToTarget(double target_latitude,double target_longitude)132 float TowedArray::BearingToTarget(double target_latitude, double target_longitude){
133
134 double array_latitude, array_longitude, array_heading, actual_bearing;
135 float bearing = -1;
136
137 if(GetLength() > 240){
138 array_heading = ReturnHeading();
139 ReturnLatLon(array_latitude, array_longitude);
140 actual_bearing = CalcBearing(array_latitude, array_longitude, target_latitude, target_longitude);
141 if(array_heading > actual_bearing){
142 actual_bearing += 360.0;
143 }
144 bearing = actual_bearing;
145 }
146 return bearing;
147 }
148
CalcBearing(double observer_latitude,double observer_longitude,double target_latitude,double target_longitude)149 float TowedArray::CalcBearing(double observer_latitude, double observer_longitude ,double target_latitude,double target_longitude){
150
151 double latdif=0, londif=0, bearing=0; //atan() needs doubles
152
153 if (observer_latitude > target_latitude){
154 latdif = observer_latitude - target_latitude;
155 }else{
156 latdif = target_latitude - observer_latitude;
157 }
158 if (observer_longitude > target_longitude){
159 londif = observer_longitude - target_longitude;
160 }else{
161 londif = target_longitude - observer_longitude;
162 }
163
164 if ((observer_longitude < target_longitude) &&
165 (observer_latitude < target_latitude)){
166 bearing = (360 - ((atan(latdif / londif) * 360) / 6.28318530717958647692));
167 }
168 if ((observer_longitude < target_longitude) &&
169 (observer_latitude > target_latitude)){
170 bearing = (0 + ((atan(latdif / londif) * 360) / 6.28318530717958647692));
171 }
172 if ((observer_longitude > target_longitude) &&
173 (observer_latitude < target_latitude)){
174 bearing = (180 + ((atan(latdif / londif) * 360) / 6.28318530717958647692));
175 }
176 if ((observer_longitude > target_longitude) &&
177 (observer_latitude > target_latitude)){
178 bearing = (180 - ((atan(latdif / londif) * 360) / 6.28318530717958647692));
179 }
180 if (londif == 0){
181 if (observer_latitude > target_latitude){
182 bearing = 90;
183 }else{
184 bearing = 270;
185 }
186 }
187 if (latdif == 0){
188 if (observer_longitude > target_longitude){
189 bearing = 180;
190 }else{
191 bearing = 0;
192 }
193 }
194 return (float)bearing;
195 }
196
RangeToTarget(double target_latitude,double target_longitude)197 double TowedArray::RangeToTarget(double target_latitude, double target_longitude)
198 {
199 //Calculates the distance in yards from the towed array, to the target.
200
201 double observer_latitude, observer_longitude;
202 double latdif = 0, londif = 0;
203
204 ReturnLatLon(observer_latitude, observer_longitude);
205
206 if (observer_latitude > target_latitude){
207 latdif = observer_latitude - target_latitude;
208 }else{
209 latdif = target_latitude - observer_latitude;
210 }
211 if (observer_longitude > target_longitude){
212 londif = observer_longitude - target_longitude;
213 }else{
214 londif = target_longitude - observer_longitude;
215 }
216 return sqrt((latdif * latdif) + (londif * londif));
217 }
218
219