RSDK-6119: implement getaccuracy (viamrobotics#3387)

components/movementsensor/accuracy.go

@@ -0,0 +1,59 @@
+package movementsensor
+
+import pb "go.viam.com/api/component/movementsensor/v1"
+
+// Accuracy defines the precision and reliability metrics of a movement sensor.
+// It includes various parameters to assess the sensor's accuracy in different dimensions.
+//
+// Fields:
+//
+// AccuracyMap: A mapping of specific measurement parameters to their accuracy values.
+// The keys are string identifiers for each measurement (e.g., "Hdop", "Vdop"),
+// and the values are their corresponding accuracy levels as float32.
+//
+// Hdop: Horizontal Dilution of Precision (HDOP) value. It indicates the level of accuracy
+// of horizontal measurements. Lower values represent higher precision.
+//
+// Vdop: Vertical Dilution of Precision (VDOP) value. Similar to HDOP, it denotes the
+// accuracy level of vertical measurements. Lower VDOP values signify better precision.
+//
+// NmeaFix: An integer value representing the quality of the NMEA fix.
+// Higher values generally indicate a better quality fix, with specific meanings depending
+// on the sensor and context. Generally a fix of 1 or 2 lends to large position errors,
+// ideally we want a fix of 4-5. Other fixes are unsuitable for outdoor navigation.
+// The meaning of each fix value is documented here
+// https://docs.novatel.com/OEM7/Content/Logs/GPGGA.htm#GPSQualityIndicators
+//
+// CompassDegreeError: The estimated error in compass readings, measured in degrees.
+// It signifies the deviation or uncertainty in the sensor's compass measurements.
+// A lower value implies a more accurate compass direction.
+type Accuracy struct {
+	AccuracyMap        map[string]float32
+	Hdop               float32
+	Vdop               float32
+	NmeaFix            int32
+	CompassDegreeError float32
+}
+
+// ProtoFeaturesToAccuracy converts a GetAccuracyResponse from a protocol buffer (protobuf)
+// into an Accuracy struct.
+func protoFeaturesToAccuracy(resp *pb.GetAccuracyResponse) *Accuracy {
+	return &Accuracy{
+		AccuracyMap:        resp.Accuracy,
+		Hdop:               *resp.PositionHdop,
+		Vdop:               *resp.PositionVdop,
+		NmeaFix:            *resp.PositionNmeaGgaFix,
+		CompassDegreeError: *resp.CompassDegreesError,
+	}
+}
+
+// AccuracyToProtoResponse converts an Accuracy struct into a protobuf GetAccuracyResponse.
+func accuracyToProtoResponse(acc *Accuracy) (*pb.GetAccuracyResponse, error) {
+	return &pb.GetAccuracyResponse{
+		Accuracy:            acc.AccuracyMap,
+		PositionHdop:        &acc.Hdop,
+		PositionVdop:        &acc.Vdop,
+		PositionNmeaGgaFix:  &acc.NmeaFix,
+		CompassDegreesError: &acc.CompassDegreeError,
+	}, nil
+}

components/movementsensor/adxl345/adxl345.go

@@ -566,8 +566,8 @@ func (adxl *adxl345) Position(ctx context.Context, extra map[string]interface{})
 	return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition
 }
 
-func (adxl *adxl345) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
-	return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy
+func (adxl *adxl345) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+	return nil, movementsensor.ErrMethodUnimplementedAccuracy
 }
 
 func (adxl *adxl345) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) {

components/movementsensor/client.go

@@ -151,7 +151,8 @@ func (c *client) Readings(ctx context.Context, extra map[string]interface{}) (ma
 	return protoutils.ReadingProtoToGo(resp.Readings)
 }
 
-func (c *client) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
+func (c *client) Accuracy(ctx context.Context, extra map[string]interface{}) (*Accuracy, error,
+) {
 	ext, err := structpb.NewStruct(extra)
 	if err != nil {
 		return nil, err
@@ -163,7 +164,7 @@ func (c *client) Accuracy(ctx context.Context, extra map[string]interface{}) (ma
 	if err != nil {
 		return nil, err
 	}
-	return resp.Accuracy, nil
+	return protoFeaturesToAccuracy(resp), nil
 }
 
 func (c *client) Properties(ctx context.Context, extra map[string]interface{}) (*Properties, error) {

components/movementsensor/client_test.go

@@ -51,7 +51,7 @@ func TestClient(t *testing.T) {
 	heading := 202.
 	props := &movementsensor.Properties{LinearVelocitySupported: true}
 	aclZ := 1.0
-	acy := map[string]float32{"x": 1.1}
+	acy := &movementsensor.Accuracy{AccuracyMap: map[string]float32{"x": 1.1}}
 	rs := map[string]interface{}{
 		"position":            loc,
 		"altitude":            alt,
@@ -82,7 +82,9 @@ func TestClient(t *testing.T) {
 	injectMovementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) {
 		return props, nil
 	}
-	injectMovementSensor.AccuracyFunc = func(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) { return acy, nil }
+	injectMovementSensor.AccuracyFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+		return acy, nil
+	}
 	injectMovementSensor.ReadingsFunc = func(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) {
 		return rs, nil
 	}

components/movementsensor/dualgps/dualgps.go

@@ -263,8 +263,8 @@ func (dg *dualGPS) Orientation(ctx context.Context, extra map[string]interface{}
 	return spatialmath.NewZeroOrientation(), movementsensor.ErrMethodUnimplementedOrientation
 }
 
-func (dg *dualGPS) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
-	return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy
+func (dg *dualGPS) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+	return nil, movementsensor.ErrMethodUnimplementedAccuracy
 }
 
 func (dg *dualGPS) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {

components/movementsensor/fake/movementsensor.go

@@ -80,8 +80,16 @@ func (f *MovementSensor) DoCommand(ctx context.Context, cmd map[string]interface
 }
 
 // Accuracy gets the accuracy of a fake movementsensor.
-func (f *MovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
-	return map[string]float32{}, nil
+func (f *MovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+	acc := movementsensor.Accuracy{
+		AccuracyMap:        map[string]float32{},
+		Hdop:               0,
+		Vdop:               0,
+		NmeaFix:            4,
+		CompassDegreeError: 0,
+	}
+
+	return &acc, nil
 }
 
 // Readings gets the readings of a fake movementsensor.

components/movementsensor/gpsnmea/pmtkI2C.go

@@ -243,10 +243,17 @@ func (g *PmtkI2CNMEAMovementSensor) Position(ctx context.Context, extra map[stri
 }
 
 // Accuracy returns the accuracy, hDOP and vDOP.
-func (g *PmtkI2CNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
+func (g *PmtkI2CNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
 	g.mu.RLock()
 	defer g.mu.RUnlock()
-	return map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.HDOP)}, g.err.Get()
+	acc := movementsensor.Accuracy{
+		AccuracyMap:        map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.VDOP)},
+		Hdop:               float32(g.data.HDOP),
+		Vdop:               float32(g.data.VDOP),
+		NmeaFix:            int32(g.data.FixQuality),
+		CompassDegreeError: float32(math.NaN()),
+	}
+	return &acc, g.err.Get()
 }
 
 // LinearVelocity returns the current speed of the MovementSensor.

components/movementsensor/gpsnmea/serial.go

@@ -174,11 +174,19 @@ func (g *SerialNMEAMovementSensor) Position(ctx context.Context, extra map[strin
 	return currentPosition, g.data.Alt, g.err.Get()
 }
 
-// Accuracy returns the accuracy, hDOP and vDOP.
-func (g *SerialNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
+// Accuracy returns the accuracy map, hDOP, vDOP, Fixquality and compass heading error.
+func (g *SerialNMEAMovementSensor) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error,
+) {
 	g.mu.RLock()
 	defer g.mu.RUnlock()
-	return map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.VDOP)}, nil
+	acc := movementsensor.Accuracy{
+		AccuracyMap:        map[string]float32{"hDOP": float32(g.data.HDOP), "vDOP": float32(g.data.VDOP)},
+		Hdop:               float32(g.data.HDOP),
+		Vdop:               float32(g.data.VDOP),
+		NmeaFix:            int32(g.data.FixQuality),
+		CompassDegreeError: float32(math.NaN()),
+	}
+	return &acc, g.err.Get()
 }
 
 // LinearVelocity linear velocity.

components/movementsensor/gpsrtkpmtk/gpsrtkpmtk.go

@@ -502,8 +502,9 @@ func (g *rtkI2C) receiveAndWriteI2C(ctx context.Context) {
 	}
 }
 
-//nolint
 // getNtripConnectionStatus returns true if connection to NTRIP stream is OK, false if not
+//
+//nolint:all
 func (g *rtkI2C) getNtripConnectionStatus() (bool, error) {
 	g.ntripMu.Lock()
 	defer g.ntripMu.Unlock()
@@ -640,10 +641,10 @@ func (g *rtkI2C) Properties(ctx context.Context, extra map[string]interface{}) (
 }
 
 // Accuracy passthrough.
-func (g *rtkI2C) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
+func (g *rtkI2C) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
 	lastError := g.err.Get()
 	if lastError != nil {
-		return map[string]float32{}, lastError
+		return nil, lastError
 	}
 
 	return g.nmeamovementsensor.Accuracy(ctx, extra)

components/movementsensor/gpsrtkserial/gpsrtkserial.go

@@ -197,7 +197,7 @@ func (g *rtkSerial) Reconfigure(ctx context.Context, deps resource.Dependencies,
 
 	g.ntripconfigMu.Unlock()
 
-	g.logger.CDebug(ctx, "done reconfiguring")
+	g.logger.Debug("done reconfiguring")
 
 	return nil
 }
@@ -426,11 +426,18 @@ func (g *rtkSerial) connectAndParseSourceTable() error {
 	}
 
 	g.logger.Debug("gettting source table")
+
+	srcReader, err := g.ntripClient.Client.GetSourcetable()
+	g.logger.Debugf("getSourceTable sends: %v\n", srcReader)
+	g.logger.Debugf("error from getsourcetable: %v\n", err)
+
 	srcTable, err := g.ntripClient.Client.ParseSourcetable()
 	if err != nil {
 		g.logger.Errorf("failed to get source table: %v", err)
 		return err
 	}
+	g.logger.Debugf("sourceTable is: %v\n", srcTable)
+
 	g.logger.Debug("got sourcetable, parsing it...")
 	g.isVirtualBase, err = rtk.FindLineWithMountPoint(srcTable, g.ntripClient.MountPoint)
 	if err != nil {
@@ -676,10 +683,11 @@ func (g *rtkSerial) Properties(ctx context.Context, extra map[string]interface{}
 }
 
 // Accuracy passthrough.
-func (g *rtkSerial) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
+func (g *rtkSerial) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error,
+) {
 	lastError := g.err.Get()
 	if lastError != nil {
-		return map[string]float32{}, lastError
+		return nil, lastError
 	}
 
 	return g.nmeamovementsensor.Accuracy(ctx, extra)

components/movementsensor/imuvectornav/imu.go

@@ -279,8 +279,8 @@ func (vn *vectornav) Position(ctx context.Context, extra map[string]interface{})
 	return nil, 0, movementsensor.ErrMethodUnimplementedPosition
 }
 
-func (vn *vectornav) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
-	return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy
+func (vn *vectornav) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+	return nil, movementsensor.ErrMethodUnimplementedAccuracy
 }
 
 func (vn *vectornav) GetMagnetometer(ctx context.Context) (r3.Vector, error) {

components/movementsensor/imuwit/imu.go

@@ -200,8 +200,9 @@ func (imu *wit) Position(ctx context.Context, extra map[string]interface{}) (*ge
 	return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition
 }
 
-func (imu *wit) Accuracy(ctx context.Context, extra map[string]interface{}) (map[string]float32, error) {
-	return map[string]float32{}, movementsensor.ErrMethodUnimplementedAccuracy
+func (imu *wit) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error,
+) {
+	return nil, movementsensor.ErrMethodUnimplementedAccuracy
 }
 
 func (imu *wit) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) {