diff --git a/pkg/api/types/common.go b/pkg/api/types/common.go index a5de76c..95b1a97 100644 --- a/pkg/api/types/common.go +++ b/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 { diff --git a/pkg/bbl/bblreader.go b/pkg/bbl/bblreader.go index cc6f5b8..6134159 100644 --- a/pkg/bbl/bblreader.go +++ b/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{} diff --git a/pkg/bltmqtt/bltmqtt.go b/pkg/bltmqtt/bltmqtt.go index 756f638..3fe2ce0 100644 --- a/pkg/bltmqtt/bltmqtt.go +++ b/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) diff --git a/pkg/inav/inav_nav_states.go b/pkg/inav/inav_nav_states.go index 51c6a7e..efad7e3 100644 --- a/pkg/inav/inav_nav_states.go +++ b/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 + } +} diff --git a/pkg/inav/inav_wp.go b/pkg/inav/inav_wp.go index 3c53797..75c54bf 100644 --- a/pkg/inav/inav_wp.go +++ b/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,8 +79,10 @@ 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) } } @@ -89,5 +90,5 @@ func WP_state(ms *mission.Mission, b types.LogItem, tgt int, nvs int) (int, int, } } act, _ := mission.ActionMap[ms.MissionItems[k].Action] - return tgt, nvs, act + return tgt, act } diff --git a/pkg/ltmgen/ltmgen.go b/pkg/ltmgen/ltmgen.go index 30b7e9d..b3919ed 100644 --- a/pkg/ltmgen/ltmgen.go +++ b/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) diff --git a/pkg/otx/otxreader.go b/pkg/otx/otxreader.go index 644e728..059f14a 100644 --- a/pkg/otx/otxreader.go +++ b/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 }