Skip to content

Commit 4af301a

Browse files
luis109luis109
committed
Merge remote-tracking branch 'LSTS/mission/tna-nppalc-v2'
2 parents acf67e7 + 7633d9e commit 4af301a

File tree

2 files changed

+233
-0
lines changed

2 files changed

+233
-0
lines changed

‎src/Monitors/NavError/Task.cmake

Whitespace-only changes.

‎src/Monitors/NavError/Task.cpp

Lines changed: 233 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,233 @@
1+
//***************************************************************************
2+
// Copyright 2007-2021 Universidade do Porto - Faculdade de Engenharia *
3+
// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) *
4+
//***************************************************************************
5+
// This file is part of DUNE: Unified Navigation Environment. *
6+
// *
7+
// Commercial Licence Usage *
8+
// Licencees holding valid commercial DUNE licences may use this file in *
9+
// accordance with the commercial licence agreement provided with the *
10+
// Software or, alternatively, in accordance with the terms contained in a *
11+
// written agreement between you and Faculdade de Engenharia da *
12+
// Universidade do Porto. For licensing terms, conditions, and further *
13+
// information contact [email protected]. *
14+
// *
15+
// Modified European Union Public Licence - EUPL v.1.1 Usage *
16+
// Alternatively, this file may be used under the terms of the Modified *
17+
// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md *
18+
// included in the packaging of this file. You may not use this work *
19+
// except in compliance with the Licence. Unless required by applicable *
20+
// law or agreed to in writing, software distributed under the Licence is *
21+
// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF *
22+
// ANY KIND, either express or implied. See the Licence for the specific *
23+
// language governing permissions and limitations at *
24+
// https://github.com/LSTS/dune/blob/master/LICENCE.md and *
25+
// http://ec.europa.eu/idabc/eupl.html. *
26+
//***************************************************************************
27+
// Author: Maria Costa *
28+
//***************************************************************************
29+
30+
// DUNE headers.
31+
#include <DUNE/DUNE.hpp>
32+
33+
namespace Monitors
34+
{
35+
//! Calculates navigation error, in meters, when a GpsFix is received.
36+
//! It also calculates the distance traveled while underwater, in meters.
37+
//! Ratio navigation error / distance traveled is also dispatched.
38+
//!
39+
//! Every time the vehicle surfaces, this monitor outputs distance messages with navigation error calculation,
40+
//! the distance traveled while underwater and the ratio navigation error / distance traveled.
41+
//! @author Maria Costa
42+
namespace NavError
43+
{
44+
using DUNE_NAMESPACES;
45+
46+
struct Arguments
47+
{
48+
//! Ignore start maneuver
49+
bool start;
50+
};
51+
52+
struct Task: public DUNE::Tasks::Task
53+
{
54+
//! Flag to signal first gpsfix
55+
bool m_init;
56+
//! Flag to signal first dpath
57+
bool m_dp_init;
58+
//! Flag to signal vehicle surfacing
59+
bool m_medium;
60+
//! Task arguments.
61+
Arguments m_args;
62+
63+
IMC::DesiredPath m_last_dpath;
64+
IMC::Distance m_distError;
65+
IMC::Distance m_distTotal;
66+
IMC::Distance m_distRatio;
67+
IMC::GpsFix m_last_gps;
68+
IMC::VehicleMedium m_last_medium;
69+
70+
//! Constructor.
71+
//! @param[in] name task name.
72+
//! @param[in] ctx context.
73+
Task(const std::string& name, Tasks::Context& ctx):
74+
DUNE::Tasks::Task(name, ctx),
75+
m_init(true),
76+
m_dp_init(true),
77+
m_medium(false)
78+
{
79+
param("Ignore start maneuver", m_args.start)
80+
.description("Ignore start maneuver for DistTotal calculations.")
81+
.defaultValue("true");
82+
83+
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
84+
85+
bind<IMC::DesiredPath>(this);
86+
bind<IMC::GpsFix>(this);
87+
bind<IMC::VehicleMedium>(this);
88+
bind<IMC::VehicleState>(this);
89+
}
90+
91+
//! Reserve entity identifiers.
92+
void
93+
onEntityReservation(void)
94+
{
95+
m_distError.setSourceEntity(reserveEntity(String::str("DistError.distance")));
96+
m_distTotal.setSourceEntity(reserveEntity(String::str("DistTotal.distance")));
97+
m_distRatio.setSourceEntity(reserveEntity(String::str("DistRatio.distance")));
98+
}
99+
100+
void
101+
consume(const IMC::VehicleMedium* msg)
102+
{
103+
if (msg->getSource() != getSystemId())
104+
return;
105+
106+
if(m_last_medium.medium == IMC::VehicleMedium::VM_UNDERWATER && msg->medium == IMC::VehicleMedium::VM_WATER)
107+
m_medium = true;
108+
else if(msg->medium == IMC::VehicleMedium::VM_UNDERWATER)
109+
m_medium = false;
110+
111+
m_last_medium = *msg;
112+
}
113+
114+
void
115+
consume(const IMC::VehicleState* msg)
116+
{
117+
if(msg->op_mode == IMC::VehicleState::VS_CALIBRATION)
118+
m_distTotal.value = 0;
119+
}
120+
121+
void
122+
consume(const IMC::DesiredPath* msg)
123+
{
124+
if (msg->getSource() != getSystemId())
125+
return;
126+
127+
if(m_dp_init)
128+
{
129+
m_last_dpath = *msg;
130+
m_distTotal.value = 0;
131+
m_dp_init = false;
132+
return;
133+
}
134+
135+
if (!(m_last_dpath.end_lat - msg->end_lat) && (m_last_dpath.end_lon - msg->end_lon))
136+
return;
137+
138+
Coordinates::WMM wmm(m_ctx.dir_cfg);
139+
IMC::EstimatedState es, last;
140+
es.lat = msg->end_lat; es.lon = msg->end_lon; es.height = wmm.height(msg->end_lat, msg->end_lon);
141+
last.lat = m_last_dpath.end_lat; last.lon = m_last_dpath.end_lon; last.height = wmm.height(m_last_dpath.end_lat, m_last_dpath.end_lon);
142+
double lat = 0, lon = 0, lat_last = 0, lon_last = 0;
143+
float hae = 0, hae_last = 0;
144+
145+
// Calculate DistTotal
146+
Coordinates::toWGS84(es, lat, lon, hae);
147+
Coordinates::toWGS84(last, lat_last, lon_last, hae_last);
148+
m_distTotal.value = m_distTotal.value + WGS84::distance(lat_last, lon_last, hae_last,
149+
lat, lon, hae);
150+
debug("Calc DistTotal = %f", m_distTotal.value);
151+
152+
if(m_args.start)
153+
{
154+
m_distTotal.value = 0;
155+
m_args.start = false;
156+
}
157+
158+
m_last_dpath = *msg;
159+
}
160+
161+
void
162+
consume(const IMC::GpsFix* msg)
163+
{
164+
if (msg->getSource() != getSystemId())
165+
return;
166+
167+
if (msg->getSourceEntity() != resolveEntity("GPS"))
168+
return;
169+
170+
// Check fix validity.
171+
if ((msg->validity & IMC::GpsFix::GFV_VALID_POS) == 0)
172+
return;
173+
174+
// Check if we have valid Horizontal Accuracy index.
175+
if (msg->validity & IMC::GpsFix::GFV_VALID_HACC && msg->hacc > 20)
176+
return;
177+
else if (msg->hdop > 2.5)
178+
return;
179+
180+
if(m_init)
181+
{
182+
m_last_gps = *msg;
183+
m_init = false;
184+
return;
185+
}
186+
187+
double lat = 0, lon = 0;
188+
float hae = 0;
189+
Coordinates::WMM wmm(m_ctx.dir_cfg);
190+
IMC::EstimatedState dp;
191+
dp.lat = m_last_dpath.end_lat; dp.lon = m_last_dpath.end_lon; dp.height = wmm.height(m_last_dpath.end_lat, m_last_dpath.end_lon);
192+
193+
// Calculate DistError
194+
if(m_medium)
195+
{
196+
war("m_medium!");
197+
Coordinates::toWGS84(dp, lat, lon, hae);
198+
m_distError.value = WGS84::distance(lat, lon, hae,
199+
msg->lat,
200+
msg->lon,
201+
msg->height);
202+
dispatch(m_distError);
203+
204+
if(m_distTotal.value > 1) // Prevents outliers when VehicleMedium intermittent
205+
{
206+
m_distRatio.value = (fp64_t) m_distError.value / m_distTotal.value;
207+
dispatch(m_distRatio);
208+
dispatch(m_distTotal);
209+
inf("Err = %f | Total = %f | Ratio = %f ", m_distError.value, m_distTotal.value, m_distRatio.value);
210+
m_distTotal.value = 0;
211+
}
212+
else
213+
m_distTotal.value = 0;
214+
m_medium = 0;
215+
}
216+
217+
m_last_gps = *msg;
218+
}
219+
220+
//! Main loop.
221+
void
222+
onMain(void)
223+
{
224+
while (!stopping())
225+
{
226+
waitForMessages(1.0);
227+
}
228+
}
229+
};
230+
}
231+
}
232+
233+
DUNE_TASK

0 commit comments

Comments
 (0)