backend: add gps_info to telemetry

backend/src/plugins/telemetry/telemetry_service_impl.h

@@ -106,6 +106,45 @@ class TelemetryServiceImpl final : public dronecore::rpc::telemetry::TelemetrySe
         return grpc::Status::OK;
     }
 
+    grpc::Status SubscribeGPSInfo(grpc::ServerContext * /* context */,
+                                  const dronecore::rpc::telemetry::SubscribeGPSInfoRequest * /* request */,
+                                  grpc::ServerWriter<rpc::telemetry::GPSInfoResponse> *writer) override
+    {
+        _telemetry.gps_info_async([this, &writer](dronecore::Telemetry::GPSInfo gps_info) {
+            auto rpc_gps_info = new dronecore::rpc::telemetry::GPSInfo();
+            rpc_gps_info->set_num_satellites(gps_info.num_satellites);
+            rpc_gps_info->set_fix_type(translateGPSFixType(gps_info.fix_type));
+
+            dronecore::rpc::telemetry::GPSInfoResponse rpc_gps_info_response;
+            rpc_gps_info_response.set_allocated_gps_info(rpc_gps_info);
+            writer->Write(rpc_gps_info_response);
+        });
+
+        _stop_future.wait();
+        return grpc::Status::OK;
+    }
+
+    dronecore::rpc::telemetry::FixType translateGPSFixType(const int fix_type) const
+    {
+        switch (fix_type) {
+            default:
+            case 0:
+                return dronecore::rpc::telemetry::FixType::NO_GPS;
+            case 1:
+                return dronecore::rpc::telemetry::FixType::NO_FIX;
+            case 2:
+                return dronecore::rpc::telemetry::FixType::FIX_2D;
+            case 3:
+                return dronecore::rpc::telemetry::FixType::FIX_3D;
+            case 4:
+                return dronecore::rpc::telemetry::FixType::FIX_DGPS;
+            case 5:
+                return dronecore::rpc::telemetry::FixType::RTK_FLOAT;
+            case 6:
+                return dronecore::rpc::telemetry::FixType::RTK_FIXED;
+        }
+    }
+
     void stop()
     {
         _stop_promise.set_value();

backend/test/telemetry_service_impl_test.cpp

@@ -25,6 +25,9 @@ using Position = dronecore::Telemetry::Position;
 using HealthResponse = dronecore::rpc::telemetry::HealthResponse;
 using Health = dronecore::Telemetry::Health;
 
+using GPSInfo = dronecore::Telemetry::GPSInfo;
+using FixType = dronecore::rpc::telemetry::FixType;
+
 class TelemetryServiceImplTest : public ::testing::Test
 {
 protected:
@@ -69,6 +72,11 @@ class TelemetryServiceImplTest : public ::testing::Test
     void checkSendsArmedEvents(const std::vector<bool> &armed_events) const;
     std::future<void> subscribeArmedAsync(std::vector<bool> &armed_events) const;
 
+    GPSInfo createGPSInfo(const int num_satellites, const int fix_type) const;
+    void checkSendsGPSInfoEvents(const std::vector<GPSInfo> &gps_info_events) const;
+    std::future<void> subscribeGPSInfoAsync(std::vector<GPSInfo> &gps_info_events) const;
+    int translateRPCGPSFixType(const FixType rpc_fix_type) const;
+
     std::unique_ptr<grpc::Server> _server;
     std::unique_ptr<TelemetryService::Stub> _stub;
     std::unique_ptr<MockTelemetry> _telemetry;
@@ -566,4 +574,127 @@ TEST_F(TelemetryServiceImplTest, sendsMultipleArmedEvents)
     checkSendsArmedEvents(armed_events);
 }
 
+TEST_F(TelemetryServiceImplTest, registersToTelemetryGPSInfoAsync)
+{
+    EXPECT_CALL(*_telemetry, gps_info_async(_))
+    .Times(1);
+
+    std::vector<GPSInfo> gps_info_events;
+    auto gps_info_stream_future = subscribeGPSInfoAsync(gps_info_events);
+
+    _telemetry_service->stop();
+    gps_info_stream_future.wait();
+}
+
+std::future<void>
+TelemetryServiceImplTest::subscribeGPSInfoAsync(std::vector<GPSInfo> &gps_info_events) const
+{
+    return std::async(std::launch::async, [&]() {
+        grpc::ClientContext context;
+        dronecore::rpc::telemetry::SubscribeGPSInfoRequest request;
+        auto response_reader = _stub->SubscribeGPSInfo(&context, request);
+
+        dronecore::rpc::telemetry::GPSInfoResponse response;
+        while (response_reader->Read(&response)) {
+            auto gps_info_rpc = response.gps_info();
+
+            GPSInfo gps_info;
+            gps_info.num_satellites = gps_info_rpc.num_satellites();
+            gps_info.fix_type = translateRPCGPSFixType(gps_info_rpc.fix_type());
+
+            gps_info_events.push_back(gps_info);
+        }
+
+        response_reader->Finish();
+    });
+}
+
+int TelemetryServiceImplTest::translateRPCGPSFixType(const FixType rpc_fix_type) const
+{
+    switch (rpc_fix_type) {
+        default:
+        case FixType::NO_GPS:
+            return 0;
+        case FixType::NO_FIX:
+            return 1;
+        case FixType::FIX_2D:
+            return 2;
+        case FixType::FIX_3D:
+            return 3;
+        case FixType::FIX_DGPS:
+            return 4;
+        case FixType::RTK_FLOAT:
+            return 5;
+        case FixType::RTK_FIXED:
+            return 6;
+    }
+}
+
+TEST_F(TelemetryServiceImplTest, doesNotSendGPSInfoIfCallbackNotCalled)
+{
+    std::vector<GPSInfo> gps_info_events;
+    auto gps_info_stream_future = subscribeGPSInfoAsync(gps_info_events);
+
+    _telemetry_service->stop();
+    gps_info_stream_future.wait();
+
+    EXPECT_EQ(0, gps_info_events.size());
+}
+
+TEST_F(TelemetryServiceImplTest, sendsOneGPSInfoEvent)
+{
+    std::vector<GPSInfo> gps_info_events;
+    gps_info_events.push_back(createGPSInfo(10, 3));
+
+    checkSendsGPSInfoEvents(gps_info_events);
+}
+
+void TelemetryServiceImplTest::checkSendsGPSInfoEvents(const std::vector<GPSInfo> &gps_info_events)
+const
+{
+    std::promise<void> subscription_promise;
+    auto subscription_future = subscription_promise.get_future();
+    dronecore::Telemetry::gps_info_callback_t gps_info_callback;
+    EXPECT_CALL(*_telemetry, gps_info_async(_))
+    .WillOnce(SaveCallback(&gps_info_callback, &subscription_promise));
+
+    std::vector<GPSInfo> received_gps_info_events;
+    auto gps_info_stream_future = subscribeGPSInfoAsync(received_gps_info_events);
+    subscription_future.wait();
+    for (const auto gps_info : gps_info_events) {
+        gps_info_callback(gps_info);
+    }
+    _telemetry_service->stop();
+    gps_info_stream_future.wait();
+
+    ASSERT_EQ(gps_info_events.size(), received_gps_info_events.size());
+    for (size_t i = 0; i < gps_info_events.size(); i++) {
+        EXPECT_EQ(gps_info_events.at(i), received_gps_info_events.at(i));
+    }
+}
+
+GPSInfo TelemetryServiceImplTest::createGPSInfo(const int num_satellites, const int fix_type) const
+{
+    GPSInfo expected_gps_info;
+
+    expected_gps_info.num_satellites = num_satellites;
+    expected_gps_info.fix_type = fix_type;
+
+    return expected_gps_info;
+}
+
+TEST_F(TelemetryServiceImplTest, sendsMultipleGPSInfoEvents)
+{
+    std::vector<GPSInfo> gps_info_events;
+    gps_info_events.push_back(createGPSInfo(5, 0));
+    gps_info_events.push_back(createGPSInfo(0, 1));
+    gps_info_events.push_back(createGPSInfo(10, 2));
+    gps_info_events.push_back(createGPSInfo(8, 3));
+    gps_info_events.push_back(createGPSInfo(22, 4));
+    gps_info_events.push_back(createGPSInfo(13, 5));
+    gps_info_events.push_back(createGPSInfo(7, 6));
+
+    checkSendsGPSInfoEvents(gps_info_events);
+}
+
 } // namespace

plugins/telemetry/mocks/telemetry_mock.h

@@ -13,6 +13,7 @@ class MockTelemetry
     MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t));
     MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t));
     MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t));
+    MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t));
 };
 
 } // namespace testing

plugins/telemetry/telemetry.cpp

@@ -338,4 +338,16 @@ std::ostream &operator<<(std::ostream &str, Telemetry::Health const &health)
            ", home_position_ok: " << health.home_position_ok << "]";
 }
 
+bool operator==(const Telemetry::GPSInfo &lhs, const Telemetry::GPSInfo &rhs)
+{
+    return lhs.num_satellites == rhs.num_satellites
+           && lhs.fix_type == rhs.fix_type;
+}
+
+std::ostream &operator<<(std::ostream &str, Telemetry::GPSInfo const &gps_info)
+{
+    return str << "[num_sat: " << gps_info.num_satellites
+           << ", fix_type: " << gps_info.fix_type << "]";
+}
+
 } // namespace dronecore

plugins/telemetry/telemetry.h

@@ -643,4 +643,7 @@ std::ostream &operator<<(std::ostream &str, Telemetry::Position const &position)
 bool operator==(const Telemetry::Health &lhs, const Telemetry::Health &rhs);
 std::ostream &operator<<(std::ostream &str, Telemetry::Health const &health);
 
+bool operator==(const Telemetry::GPSInfo &lhs, const Telemetry::GPSInfo &rhs);
+std::ostream &operator<<(std::ostream &str, Telemetry::GPSInfo const &gps_info);
+
 } // namespace dronecore