-
Notifications
You must be signed in to change notification settings - Fork 690
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
mavproxy_soar: Add initial soaring display
Signed-off-by: Ryan Friedman <[email protected]>
- Loading branch information
1 parent
7aecc5a
commit 2b1803c
Showing
1 changed file
with
96 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,96 @@ | ||
#!/usr/bin/env python3 | ||
''' | ||
Soaring Module | ||
Ryan Friedman | ||
This module displays the estimated soaring thermals on the map. | ||
A circle is drawn with the estimated radius from ArduSoar's estimated thermal location. | ||
Note that this module uses NAMED_VALUE_FLOAT messages from the autopilot, | ||
a temporary measure until a proper mavlink message is decided upon. | ||
AP_FLAKE8_CLEAN | ||
''' | ||
|
||
from MAVProxy.modules.lib import mp_module, mp_util | ||
from MAVProxy.modules.mavproxy_map import mp_slipmap | ||
|
||
import time | ||
|
||
|
||
class soar(mp_module.MPModule): | ||
_SOAR_THERMAL_MAP_OBJ_ID = "soar-thermal" | ||
_CLEAR_STALE_THERMAL_TIME = 5.0 | ||
|
||
def __init__(self, mpstate): | ||
"""Initialise module""" | ||
super(soar, self).__init__(mpstate, "soar", "") | ||
self._strength = None | ||
self._radius = None | ||
self._x = None | ||
self._y = None | ||
self._last_draw_time = time.time() | ||
|
||
def mavlink_packet(self, m): | ||
'''handle mavlink packets''' | ||
|
||
if m.get_type() == 'NAMED_VALUE_FLOAT' and m.name.startswith("SOAR"): | ||
if m.name == "SOAREKFX0": | ||
self._strength = m.value | ||
elif m.name == "SOAREKFX1": | ||
self._radius = m.value | ||
elif m.name == "SOAREKFX2": | ||
self._x = m.value | ||
elif m.name == "SOAREKFX3": | ||
self._y = m.value | ||
else: | ||
raise NotImplementedError(m.name) | ||
|
||
self.draw_thermal_estimate() | ||
|
||
def idle_task(self): | ||
'''called rapidly by mavproxy''' | ||
|
||
if time.time() - self._last_draw_time > self._CLEAR_STALE_THERMAL_TIME: | ||
self.clear_thermal_estimate() | ||
|
||
def draw_thermal_estimate(self): | ||
|
||
if self._radius is None: | ||
return | ||
if self._x is None: | ||
return | ||
if self._y is None: | ||
return | ||
|
||
wp_module = self.module('wp') | ||
if wp_module is None: | ||
return | ||
home = wp_module.get_home() | ||
if home is None: | ||
return | ||
|
||
home_lat = home.x | ||
home_lng = home.y | ||
|
||
(thermal_lat, thermal_lon) = mp_util.gps_offset(home_lat, home_lng, self._y, self._x) | ||
|
||
slipcircle = mp_slipmap.SlipCircle( | ||
self._SOAR_THERMAL_MAP_OBJ_ID, # key | ||
"thermals", # layer | ||
(thermal_lat, thermal_lon), # latlon | ||
self._radius, # radius | ||
(0, 255, 255), | ||
linewidth=2) | ||
for mp in self.module_matching('map*'): | ||
self._last_draw_time = time.time() | ||
mp.map.add_object(slipcircle) | ||
|
||
def clear_thermal_estimate(self): | ||
for mp in self.module_matching('map*'): | ||
mp.map.remove_object(self._SOAR_THERMAL_MAP_OBJ_ID) | ||
|
||
|
||
def init(mpstate): | ||
'''initialise module''' | ||
return soar(mpstate) |