improve "navmode" calculation, always get the headers from the actual…

… index, as they may change

pkg/api/types/common.go

@@ -76,7 +76,7 @@ type LogItem struct {
 	Status   uint8
 	Hdop     uint16
 	HWfail   bool
-	NavState int
+	NavMode  byte
 }
 
 type LogRec struct {

pkg/bbl/bblreader.go

@@ -62,6 +62,10 @@ func get_headers(fn string) {
 		os.Exit(1)
 	}
 	record, err := r.Read()
+	build_headers(record)
+}
+
+func build_headers(record []string) {
 	hdrs = make(map[string]int)
 	for i, s := range record {
 		hdrs[s] = i
@@ -70,7 +74,6 @@ func get_headers(fn string) {
 		fmt.Fprintln(os.Stderr, "No \"datetime\" header, probably blackbox_decode too old or broken")
 		os.Exit(1)
 	}
-
 }
 
 func dump_headers() {
@@ -383,7 +386,6 @@ func get_bbl_line(r []string, have_origin bool) types.LogItem {
 	s0, sok := get_rec_value(r, "flightModeFlags (flags)")
 	if s, ok = get_rec_value(r, "navState"); ok {
 		i64, _ := strconv.ParseInt(s, 10, 64)
-		b.NavState = int(i64)
 		if inav.IsCruise3d(inav_vers, int(i64)) {
 			md = types.FM_CRUISE3D
 		} else if inav.IsCruise2d(inav_vers, int(i64)) {
@@ -409,8 +411,7 @@ func get_bbl_line(r []string, have_origin bool) types.LogItem {
 				md = types.FM_HORIZON
 			}
 		}
-	} else {
-		b.NavState = -1
+		b.NavMode = inav.NavMode(inav_vers, int(i64))
 	}
 	// fallback for old inav bug
 	if sok && strings.Contains(s0, "NAVRTH") {
@@ -567,6 +568,7 @@ func (lg *BBLOG) Reader(meta types.FlightMeta) (types.LogSegment, bool) {
 			break
 		}
 		if i == 0 {
+			build_headers(record)
 			rec.Cap = dataCapability()
 			continue
 		}
@@ -682,7 +684,6 @@ func (lg *BBLOG) Reader(meta types.FlightMeta) (types.LogSegment, bool) {
 			log.Fatal(err)
 		}
 	}
-
 	srec := stats.Summary(lt - st)
 	ok := homes.Flags != 0 && len(rec.Items) > 0
 	ls := types.LogSegment{}

pkg/bltmqtt/bltmqtt.go

@@ -158,7 +158,7 @@ func (m *MQTTClient) publish(msg string) {
    broker.emqx.io    1883, 8883, 8083, 8084 (ws)
 */
 
-func make_bullet_msg(b types.LogItem, homeamsl float64, elapsed int, ncells int, tgt int, nvs int) string {
+func make_bullet_msg(b types.LogItem, homeamsl float64, elapsed int, ncells int, tgt int) string {
 	var sb strings.Builder
 
 	sb.WriteString("flt:")
@@ -291,7 +291,7 @@ func make_bullet_msg(b types.LogItem, homeamsl float64, elapsed int, ncells int,
 	sb.WriteByte(',')
 
 	if tgt != 0 {
-		sb.WriteString(fmt.Sprintf("cwn:%d,nvs:%d,", tgt, nvs))
+		sb.WriteString(fmt.Sprintf("cwn:%d,nvs:%d,", tgt, b.NavMode))
 	}
 
 	armed := b.Status & 1
@@ -372,7 +372,6 @@ func MQTTGen(s types.LogSegment, meta types.FlightMeta) {
 	ncells := 0
 	var wfh *os.File
 	tgt := 0
-	nvs := 0
 	var name string
 	if meta.Flags&types.Has_Craft != 0 {
 		name = meta.Craft
@@ -466,7 +465,6 @@ func MQTTGen(s types.LogSegment, meta types.FlightMeta) {
 
 		if b.Fmode != laststat {
 			tgt = 0
-			nvs = 0
 			if options.Bulletvers == 2 {
 				switch b.Fmode {
 				case types.FM_MANUAL:
@@ -482,17 +480,14 @@ func MQTTGen(s types.LogSegment, meta types.FlightMeta) {
 				case types.FM_PH:
 					fmode = "4"
 					tgt = 0
-					nvs = 3
 				case types.FM_WP:
 					fmode = "7"
 					if ms != nil {
 						tgt = 1
-						nvs = 5
 					}
 				case types.FM_RTH:
 					fmode = "2"
 					tgt = 0
-					nvs = 1
 				case types.FM_CRUISE3D:
 					fmode = "5"
 				case types.FM_LAUNCH:
@@ -531,7 +526,7 @@ func MQTTGen(s types.LogSegment, meta types.FlightMeta) {
 			}
 			msg := make_bullet_mode(fmode, ncells, b.HWfail)
 			output_message(c, wfh, msg, b.Utc)
-			output_message(c, wfh, fmt.Sprintf("cwn:%d,nvs:%d", tgt, nvs), b.Utc)
+			output_message(c, wfh, fmt.Sprintf("cwn:%d,nvs:%d", tgt, b.NavMode), b.Utc)
 			laststat = b.Fmode
 		}
 
@@ -557,9 +552,9 @@ func MQTTGen(s types.LogSegment, meta types.FlightMeta) {
 		}
 
 		if b.Fmode == types.FM_WP && ms != nil {
-			tgt, nvs, _ = inav.WP_state(ms, b, tgt, nvs)
+			tgt, _ = inav.WP_state(ms, b, tgt)
 		}
-		msg := make_bullet_msg(b, s.H.HomeAlt, et, ncells, tgt, nvs)
+		msg := make_bullet_msg(b, s.H.HomeAlt, et, ncells, tgt)
 		output_message(c, wfh, msg, b.Utc)
 		if c != nil && !lastm.IsZero() {
 			tdiff := b.Utc.Sub(lastm)

pkg/inav/inav_nav_states.go

@@ -94,3 +94,82 @@ func IsEmerg(vers, val int) bool {
 		return contains([]int{27, 28, 29}, val)
 	}
 }
+
+func is_rth_start(vers, val int) bool {
+	return val == 8
+}
+
+func is_start_land(vers, val int) bool {
+	switch {
+	case vers > 0x10601: // For 1.6.2, hex = 0x10601
+		return contains([]int{11, 22}, val)
+	case vers > 0x105ff: // 1.6 and later
+		return contains([]int{16, 27}, val)
+	case vers > 0x101ff: // 1.2 and later
+		return contains([]int{18, 29}, val)
+	default: // prior to 1.2
+		return contains([]int{18, 27}, val)
+	}
+	return false
+}
+
+func is_landing(vers, val int) bool {
+	switch {
+	case vers > 0x10601: // For 1.6.2, hex = 0x10601
+		return contains([]int{12, 21, 23}, val)
+	case vers > 0x105ff: // 1.6 and later
+		return contains([]int{17, 26, 28}, val)
+	case vers > 0x101ff: // 1.2 and later
+		return contains([]int{19, 28, 30}, val)
+	default: // prior to 1.2
+		return contains([]int{19, 28}, val)
+	}
+	return false
+}
+
+func is_landed(vers, val int) bool {
+	switch {
+	case vers > 0x10601: // For 1.6.2
+		return val == 24
+	case vers > 0x105ff: // 1.6.0
+		return val == 29
+	case vers > 0x101ff: // For 1.2.0
+		return val == 31
+	default:
+		return val == 29
+	}
+
+}
+
+func is_hover(vers, val int) bool {
+	switch {
+	case vers > 0x206ff: // 2.7.0 and later
+		return contains([]int{11, 36, 37}, val)
+	case vers > 0x10601: // For 1.6.2
+		return val == 11
+	case vers > 0x105ff: // 1.6.0
+		return val == 16
+	default:
+		return val == 18
+	}
+}
+
+func NavMode(vers, val int) byte {
+	if is_rth_start(vers, val) {
+		return 1
+	} else if IsPH(vers, val) {
+		return 3
+	} else if IsWP(vers, val) {
+		return 5
+	} else if is_start_land(vers, val) {
+		return 8
+	} else if is_landing(vers, val) {
+		return 9
+	} else if is_landed(vers, val) {
+		return 10
+	} else if is_hover(vers, val) {
+		return 13
+	} else {
+		return 0
+	}
+}

pkg/inav/inav_wp.go

@@ -12,8 +12,7 @@ import (
 
 var phtime time.Time
 
-func get_next_wp(ms *mission.Mission, k int) (int, int) {
-	nvs := 0
+func get_next_wp(ms *mission.Mission, k int) int {
 	tgt := 0
 	if ms != nil && k < len(ms.MissionItems)-1 {
 		switch ms.MissionItems[k+1].Action {
@@ -31,28 +30,29 @@ func get_next_wp(ms *mission.Mission, k int) (int, int) {
 					ms.MissionItems[k+1].P3 -= 1
 				}
 			}
-			nvs = 5
 		case "RTH":
-			nvs = 1
 			tgt = int(ms.MissionItems[k+1].No)
 		case "SET_HEAD", "SET_POI":
 			if k < len(ms.MissionItems)-1 {
 				tgt = int(ms.MissionItems[k+2].No)
 			}
-			nvs = 5
 		default:
 			tgt = int(ms.MissionItems[k+1].No)
-			nvs = 5
 		}
 	}
-	return tgt, nvs
+	return tgt
 }
 
-func WP_state(ms *mission.Mission, b types.LogItem, tgt int, nvs int) (int, int, int) {
+var isTimed bool
+
+func WP_state(ms *mission.Mission, b types.LogItem, tgt int) (int, int) {
 	k := tgt - 1
-	if nvs == 4 {
+	if isTimed {
 		if b.Utc.After(phtime) {
-			tgt, nvs = get_next_wp(ms, k)
+			tgt = get_next_wp(ms, k)
+			isTimed = false
+		} else {
+			b.NavMode = 4
 		}
 	} else {
 		cdist := 1.25 * b.Spd * float64(options.Intvl/1000.0)
@@ -71,7 +71,6 @@ func WP_state(ms *mission.Mission, b types.LogItem, tgt int, nvs int) (int, int,
 				if bdiff > 90 || bdiff < -90 {
 					//					fmt.Fprintf(os.Stderr, "Reached %d %s\n", k, ms.MissionItems[k].Action)
 					if ms.MissionItems[k].Action == "POSHOLD_TIME" {
-						nvs = 4
 						var phwait time.Duration
 						mwaitms := int(ms.MissionItems[k].P1) * 1000
 						if mwaitms > options.Intvl/2000 {
@@ -80,14 +79,16 @@ func WP_state(ms *mission.Mission, b types.LogItem, tgt int, nvs int) (int, int,
 							phwait = time.Duration(ms.MissionItems[k].P1) * time.Second
 						}
 						phtime = b.Utc.Add(phwait)
+						isTimed = true
+						b.NavMode = 4
 					} else {
-						tgt, nvs = get_next_wp(ms, k)
+						tgt = get_next_wp(ms, k)
 						//						fmt.Fprintf(os.Stderr, "New target WP %d %d (%s)\n", tgt, nvs, ms.MissionItems[k+1].Action)
 					}
 				}
 			}
 		}
 	}
 	act, _ := mission.ActionMap[ms.MissionItems[k].Action]
-	return tgt, nvs, act
+	return tgt, act
 }

pkg/ltmgen/ltmgen.go

@@ -99,11 +99,8 @@ func (l *ltmbuf) nframe(b types.LogItem, action byte, wpno byte) {
 	default:
 		l.msg[3] = 0
 	}
-	if b.NavState != -1 {
-		l.msg[4] = byte(b.NavState)
-	} else {
-		l.msg[4] = 0 // synthesise
-	}
+
+	l.msg[4] = b.NavMode
 	l.msg[7] = 0
 	l.msg[8] = 0
 	l.checksum()
@@ -158,9 +155,13 @@ func (l *ltmbuf) sframe(b types.LogItem) {
 	l.checksum()
 }
 
-func (l *ltmbuf) xframe(hdop uint16, xcount uint8) {
-	binary.LittleEndian.PutUint16(l.msg[3:5], hdop)
-	l.msg[5] = 0
+func (l *ltmbuf) xframe(b types.LogItem, xcount uint8) {
+	binary.LittleEndian.PutUint16(l.msg[3:5], b.Hdop)
+	if b.HWfail {
+		l.msg[5] = 1
+	} else {
+		l.msg[5] = 0
+	}
 	l.msg[6] = xcount
 	l.msg[7] = 0
 	l.checksum()
@@ -219,9 +220,8 @@ func LTMGen(seg types.LogSegment, meta types.FlightMeta) {
 	s = NewMSPSerial(options.LTMdev, 0)
 
 	laststat := uint8(255)
-	nvs := 0
 	tgt := 0
-	xnvs := 0
+	xnvs := byte(0)
 	xtgt := 0
 
 	xcount := uint8(0)
@@ -289,21 +289,15 @@ func LTMGen(seg types.LogSegment, meta types.FlightMeta) {
 			case types.FM_WP:
 				if ms != nil {
 					tgt = 1
-					nvs = 5
 				}
 			case types.FM_RTH:
 				tgt = 0
-				nvs = 1
 			case types.FM_PH:
 				tgt = 0
-				nvs = 3
 			default:
 				tgt = 0
-				nvs = 0
 			}
-			//			if b.NavState == -1 {
-			b.NavState = nvs
-			//			}
+
 			l := newLTM('N')
 			l.nframe(b, 0, 0)
 			s.Write(l.msg)
@@ -314,9 +308,8 @@ func LTMGen(seg types.LogSegment, meta types.FlightMeta) {
 
 		if b.Fmode == types.FM_WP && ms != nil {
 			act := 0
-			tgt, nvs, act = inav.WP_state(ms, b, tgt, nvs)
-			if tgt != xtgt || nvs != xnvs {
-				b.NavState = nvs
+			tgt, act = inav.WP_state(ms, b, tgt)
+			if tgt != xtgt || b.NavMode != xnvs {
 				l := newLTM('N')
 				l.nframe(b, byte(act), byte(tgt))
 				s.Write(l.msg)
@@ -345,7 +338,7 @@ func LTMGen(seg types.LogSegment, meta types.FlightMeta) {
 			l.oframe(b, hlat, hlon)
 			s.Write(l.msg)
 			l = newLTM('X')
-			l.xframe(b.Hdop, xcount)
+			l.xframe(b, xcount)
 			s.Write(l.msg)
 			xcount = (xcount + 1) & 0xff
 			g3t = b.Utc.Add(g3diff)

pkg/otx/otxreader.go

@@ -461,7 +461,6 @@ func get_otx_line(r []string) types.LogItem {
 	}
 	b.Throttle = 100 * (b.Throttle + 1024) / 2048
 	b.Status = status
-	b.NavState = -1
 	return b
 }