Add set attitude command

Author: alielbashir

server/commands.go

@@ -2,6 +2,8 @@ package server
 
 import (
 	"context"
+	"time"
+
 	"github.com/aler9/gomavlib/pkg/dialects/ardupilotmega"
 	pb "github.com/koustech/mastermind/gen/go/proto/mastermind/v1"
 	"github.com/koustech/mastermind/utils"
@@ -85,3 +87,43 @@ func (s *mastermindServiceServer) GotoWaypoint(_ context.Context, request *pb.Go
 	return &pb.GotoWaypointResponse{CommandSucceeded: true}, nil
 
 }
+
+const SET_ATTITUDE_TIMEOUT_SECONDS = 6
+
+// SetAttitude tells the plane to obtain a specific attitude
+func (s *mastermindServiceServer) SetAttitude(stream pb.MastermindService_SetAttitudeServer) error {
+	// track time for case of timeout
+	startTime := time.Now()
+	utils.Logger.Info("SetAttitude() called")
+
+	// set mode to FBWB ONE TIME ONLY
+	// TODO: implement
+	// for every received message, send command and update target object (to be sent in detailed telem)
+
+	for {
+		currentTime := time.Now()
+		if currentTime.Sub(startTime).Seconds() > 6 {
+			break
+		}
+
+		attitudeRequest, err := stream.Recv()
+
+		if err != nil {
+			break
+		}
+
+		s.targetMu.Lock()
+		s.target = attitudeRequest.Target
+		s.targetMu.Unlock()
+	}
+
+	// close stream once EOF is received and set target to nil
+	utils.Logger.Info("SetAttitude() finished")
+
+	s.targetMu.Lock()
+	s.target = nil
+	s.targetMu.Unlock()
+
+	return stream.SendAndClose(&pb.SetAttitudeResponse{})
+
+}

server/server.go

@@ -49,7 +49,8 @@ type mastermindServiceServer struct {
 	node                *gomavlib.Node // node connected to the plane
 	sysId               uint8          // MAVLink system ID of the plane
 	compId              uint8          // MAVLink component ID of the autopilot
-	target              pb.Target     // Target being followed by tracking service
+	target              *pb.Target     // Target being followed by tracking service
+	targetMu            sync.Mutex     // protects target
 }
 
 func NewMastermindServiceServer(node *gomavlib.Node, sysId uint8, compId uint8) *mastermindServiceServer {

server/telemetry.go

@@ -140,6 +140,10 @@ func (s *mastermindServiceServer) GetDetailedTelemetry(_ *pb.GetDetailedTelemetr
 	s.telemUpdateHandlers[sessionId] = s.stateBus.Subscribe(EventNewTelemetry, func(e evbus.Event) {
 		se := e.(*NewTelemetryEvent)
 		response := se.response
+		s.targetMu.Lock()
+		se.response.Target = s.target
+		s.targetMu.Unlock()
+
 		if err := stream.Send(response); err != nil {
 			s.stateBus.Unsubscribe(s.telemUpdateHandlers[sessionId])
 			delete(s.telemUpdateHandlers, sessionId)