[RSDK-7112] fix compass heading sensor function (#3748)

viamrobotics · Mar 29, 2024 · f9db97b · f9db97b
1 parent dfb590c
commit f9db97b
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/components/base/sensorcontrolled/movestraight.go b/components/base/sensorcontrolled/movestraight.go
@@ -165,6 +165,8 @@ func calcSlowDownDist(distanceMm int) float64 {
 func (sb *sensorBase) determineHeadingFunc(ctx context.Context) {
 	switch {
 	case sb.orientation != nil:
+		sb.logger.CInfof(ctx, "using orientation for base %v's MoveStraight heading control",
+			sb.Name().ShortName())
 		sb.headingFunc = func(ctx context.Context) (float64, error) {
 			orient, err := sb.orientation.Orientation(ctx, nil)
 			if err != nil {
@@ -176,11 +178,16 @@ func (sb *sensorBase) determineHeadingFunc(ctx context.Context) {
 			return yaw, nil
 		}
 	case sb.compassHeading != nil:
+		sb.logger.CInfof(ctx, "using compass heading for base %v's MoveStraight heading control",
+			sb.Name().ShortName())
 		sb.headingFunc = func(ctx context.Context) (float64, error) {
 			compassHeading, err := sb.compassHeading.CompassHeading(ctx, nil)
 			if err != nil {
 				return 0, err
 			}
+			// flip compass heading to be CCW/Z up
+			compassHeading = 360 - compassHeading
+
 			// make the compass heading (-180->180)
 			if compassHeading > 180 {
 				compassHeading -= 360

diff --git a/components/base/sensorcontrolled/sensorcontrolled_test.go b/components/base/sensorcontrolled/sensorcontrolled_test.go
@@ -323,7 +323,7 @@ func TestReconfig(t *testing.T) {
 	headingCompass, err := sb.headingFunc(context.Background())
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, headingCompass, test.ShouldNotEqual, orientationValue)
-	test.That(t, headingCompass, test.ShouldEqual, compassValue)
+	test.That(t, headingCompass, test.ShouldEqual, -compassValue)
 
 	deps, cfg = msDependencies(t, []string{"Bad"})
 	err = b.Reconfigure(ctx, deps, cfg)