support safehome for mqtt generation

stronnag · Feb 24, 2021 · e6f89e6 · e6f89e6
1 parent 1aec330
commit e6f89e6
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/pkg/bbl/bblreader.go b/pkg/bbl/bblreader.go
@@ -515,6 +515,7 @@ func (lg *BBLOG) Reader(meta types.FlightMeta) (types.LogSegment, bool) {
 				homes.Flags = types.HOME_ARM | types.HOME_ALT
 				if b.Bearing == -2 {
 					_, dh := geo.Csedist(b.Hlat, b.Hlon, b.Lat, b.Lon)
+
 					if dh > 2.0/1852.0 {
 						homes.SafeLat = b.Hlat
 						homes.SafeLon = b.Hlon

diff --git a/pkg/bltmqtt/bltmqtt.go b/pkg/bltmqtt/bltmqtt.go
@@ -565,7 +565,16 @@ func MQTTGen(s types.LogSegment) {
 		if i%miscout == 0 {
 			msg := make_bullet_mode(fmode, ncells, b.HWfail)
 			output_message(c, wfh, msg, b.Utc)
-			msg = make_bullet_home(s.H.HomeLat, s.H.HomeLon, s.H.HomeAlt)
+			var hlat, hlon float64
+			if s.H.Flags&types.HOME_SAFE != 0 {
+				hlat = s.H.SafeLat
+				hlon = s.H.SafeLon
+			} else {
+				hlat = s.H.HomeLat
+				hlon = s.H.HomeLon
+			}
+
+			msg = make_bullet_home(hlat, hlon, s.H.HomeAlt)
 			output_message(c, wfh, msg, b.Utc)
 			if len(mstrs) > 0 && i%2*miscout == 0 {
 				for _, str := range mstrs {