Skip to content

Commit

Permalink
AngularSpeed -> AngularVelocityBody
Browse files Browse the repository at this point in the history
  • Loading branch information
@irsdkv
irsdkv committed Aug 14, 2019
1 parent 26b81fe commit 6351b11
Show file tree
Hide file tree
Showing 9 changed files with 157 additions and 136 deletions.
39 changes: 21 additions & 18 deletions src/backend/src/plugins/telemetry/telemetry_service_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -314,26 +314,29 @@ class TelemetryServiceImpl final : public mavsdk::rpc::telemetry::TelemetryServi
return grpc::Status::OK;
}

grpc::Status SubscribeAttitudeAngularSpeed(
grpc::Status SubscribeAttitudeAngularVelocityBody(
grpc::ServerContext* /* context */,
const mavsdk::rpc::telemetry::SubscribeAttitudeAngularSpeedRequest* /* request */,
grpc::ServerWriter<rpc::telemetry::AttitudeAngularSpeedResponse>* writer) override
const mavsdk::rpc::telemetry::SubscribeAttitudeAngularVelocityBodyRequest* /* request */,
grpc::ServerWriter<rpc::telemetry::AttitudeAngularVelocityBodyResponse>* writer) override
{
std::mutex attitude_angular_speed_mutex{};

_telemetry.attitude_angular_speed_async([&writer, &attitude_angular_speed_mutex](
mavsdk::Telemetry::AngularSpeed angular_speed) {
auto rpc_angular_speed = new mavsdk::rpc::telemetry::AngularSpeed();
rpc_angular_speed->set_rollspeed(angular_speed.rollspeed);
rpc_angular_speed->set_pitchspeed(angular_speed.pitchspeed);
rpc_angular_speed->set_yawspeed(angular_speed.yawspeed);

mavsdk::rpc::telemetry::AttitudeAngularSpeedResponse rpc_angular_speed_response;
rpc_angular_speed_response.set_allocated_attitude_angular_speed(rpc_angular_speed);

std::lock_guard<std::mutex> lock(attitude_angular_speed_mutex);
writer->Write(rpc_angular_speed_response);
});
std::mutex attitude_angular_velocity_body_mutex{};

_telemetry.attitude_angular_velocity_body_async(
[&writer, &attitude_angular_velocity_body_mutex](
mavsdk::Telemetry::AngularVelocityBody angular_velocity_body) {
auto rpc_angular_velocity_body = new mavsdk::rpc::telemetry::AngularVelocityBody();
rpc_angular_velocity_body->set_roll_rad_s(angular_velocity_body.roll_rad_s);
rpc_angular_velocity_body->set_pitch_rad_s(angular_velocity_body.pitch_rad_s);
rpc_angular_velocity_body->set_yaw_rad_s(angular_velocity_body.yaw_rad_s);

mavsdk::rpc::telemetry::AttitudeAngularVelocityBodyResponse
rpc_angular_velocity_body_response;
rpc_angular_velocity_body_response.set_allocated_attitude_angular_velocity_body(
rpc_angular_velocity_body);

std::lock_guard<std::mutex> lock(attitude_angular_velocity_body_mutex);
writer->Write(rpc_angular_velocity_body_response);
});

_stop_future.wait();
return grpc::Status::OK;
Expand Down
121 changes: 63 additions & 58 deletions src/backend/test/telemetry_service_impl_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ using Quaternion = mavsdk::Telemetry::Quaternion;

using EulerAngle = mavsdk::Telemetry::EulerAngle;

using AngularSpeed = mavsdk::Telemetry::AngularSpeed;
using AngularVelocityBody = mavsdk::Telemetry::AngularVelocityBody;

using GroundSpeedNed = mavsdk::Telemetry::GroundSpeedNED;

Expand Down Expand Up @@ -105,11 +105,12 @@ class TelemetryServiceImplTest : public ::testing::Test {
Quaternion createQuaternion(const float w, const float x, const float y, const float z) const;
std::future<void> subscribeAttitudeQuaternionAsync(std::vector<Quaternion>& quaternions) const;

void checkSendsAttitudeAngularSpeeds(const std::vector<AngularSpeed>& angular_speeds) const;
AngularSpeed
createAngularSpeed(const float rollspeed, const float pitchspeed, const float yawspeed) const;
std::future<void>
subscribeAttitudeAngularSpeedAsync(std::vector<AngularSpeed>& angular_speeds) const;
void checkSendsAttitudeAngularVelocitiesBody(
const std::vector<AngularVelocityBody>& angular_velocities_body) const;
AngularVelocityBody createAngularVelocityBody(
const float roll_rad_s, const float pitch_rad_s, const float yaw_rad_s) const;
std::future<void> subscribeAttitudeAngularVelocityBodyAsync(
std::vector<AngularVelocityBody>& angular_velocities_body) const;

void checkSendsAttitudeEulerAngles(const std::vector<EulerAngle>& euler_angles) const;
EulerAngle
Expand Down Expand Up @@ -989,15 +990,16 @@ TEST_F(TelemetryServiceImplTest, registersToTelemetryAttitudeQuaternionAsync)
quaternion_stream_future.wait();
}

TEST_F(TelemetryServiceImplTest, registersToTelemetryAttitudeAngularSpeedAsync)
TEST_F(TelemetryServiceImplTest, registersToTelemetryAttitudeAngularVelocityBodyAsync)
{
EXPECT_CALL(*_telemetry, attitude_angular_speed_async(_)).Times(1);
EXPECT_CALL(*_telemetry, attitude_angular_velocity_body_async(_)).Times(1);

std::vector<AngularSpeed> angular_speeds;
auto angular_speed_stream_future = subscribeAttitudeAngularSpeedAsync(angular_speeds);
std::vector<AngularVelocityBody> angular_velocities_body;
auto angular_velocity_body_stream_future =
subscribeAttitudeAngularVelocityBodyAsync(angular_velocities_body);

_telemetry_service->stop();
angular_speed_stream_future.wait();
angular_velocity_body_stream_future.wait();
}

std::future<void> TelemetryServiceImplTest::subscribeAttitudeQuaternionAsync(
Expand Down Expand Up @@ -1036,39 +1038,40 @@ TEST_F(TelemetryServiceImplTest, doesNotSendAttitudeQuaternionIfCallbackNotCalle
EXPECT_EQ(0, quaternions.size());
}

std::future<void> TelemetryServiceImplTest::subscribeAttitudeAngularSpeedAsync(
std::vector<AngularSpeed>& angular_speeds) const
std::future<void> TelemetryServiceImplTest::subscribeAttitudeAngularVelocityBodyAsync(
std::vector<AngularVelocityBody>& angular_velocities_body) const
{
return std::async(std::launch::async, [&]() {
grpc::ClientContext context;
mavsdk::rpc::telemetry::SubscribeAttitudeAngularSpeedRequest request;
auto response_reader = _stub->SubscribeAttitudeAngularSpeed(&context, request);
mavsdk::rpc::telemetry::SubscribeAttitudeAngularVelocityBodyRequest request;
auto response_reader = _stub->SubscribeAttitudeAngularVelocityBody(&context, request);

mavsdk::rpc::telemetry::AttitudeAngularSpeedResponse response;
mavsdk::rpc::telemetry::AttitudeAngularVelocityBodyResponse response;
while (response_reader->Read(&response)) {
auto angular_speed_rpc = response.attitude_angular_speed();
auto angular_velocity_body_rpc = response.attitude_angular_velocity_body();

AngularSpeed angular_speed;
angular_speed.rollspeed = angular_speed_rpc.rollspeed();
angular_speed.pitchspeed = angular_speed_rpc.pitchspeed();
angular_speed.yawspeed = angular_speed_rpc.yawspeed();
AngularVelocityBody angular_velocity_body;
angular_velocity_body.roll_rad_s = angular_velocity_body_rpc.roll_rad_s();
angular_velocity_body.pitch_rad_s = angular_velocity_body_rpc.pitch_rad_s();
angular_velocity_body.yaw_rad_s = angular_velocity_body_rpc.yaw_rad_s();

angular_speeds.push_back(angular_speed);
angular_velocities_body.push_back(angular_velocity_body);
}

response_reader->Finish();
});
}

TEST_F(TelemetryServiceImplTest, doesNotSendAttitudeAngularSpeedIfCallbackNotCalled)
TEST_F(TelemetryServiceImplTest, doesNotSendAttitudeAngularVelocityBodyIfCallbackNotCalled)
{
std::vector<AngularSpeed> angular_speeds;
auto angular_speed_stream_future = subscribeAttitudeAngularSpeedAsync(angular_speeds);
std::vector<AngularVelocityBody> angular_velocities_body;
auto angular_velocity_body_stream_future =
subscribeAttitudeAngularVelocityBodyAsync(angular_velocities_body);

_telemetry_service->stop();
angular_speed_stream_future.wait();
angular_velocity_body_stream_future.wait();

EXPECT_EQ(0, angular_speeds.size());
EXPECT_EQ(0, angular_velocities_body.size());
}

TEST_F(TelemetryServiceImplTest, sendsOneAttitudeQuaternion)
Expand All @@ -1079,12 +1082,12 @@ TEST_F(TelemetryServiceImplTest, sendsOneAttitudeQuaternion)
checkSendsAttitudeQuaternions(quaternions);
}

TEST_F(TelemetryServiceImplTest, sendsOneAttitudeAngularSpeed)
TEST_F(TelemetryServiceImplTest, sendsOneAttitudeAngularVelocityBody)
{
std::vector<AngularSpeed> angular_speed;
angular_speed.push_back(createAngularSpeed(0.1f, 0.2f, 0.3f));
std::vector<AngularVelocityBody> angular_velocity_body;
angular_velocity_body.push_back(createAngularVelocityBody(0.1f, 0.2f, 0.3f));

checkSendsAttitudeAngularSpeeds(angular_speed);
checkSendsAttitudeAngularVelocitiesBody(angular_velocity_body);
}

Quaternion TelemetryServiceImplTest::createQuaternion(
Expand All @@ -1100,16 +1103,16 @@ Quaternion TelemetryServiceImplTest::createQuaternion(
return quaternion;
}

AngularSpeed TelemetryServiceImplTest::createAngularSpeed(
const float rollspeed, const float pitchspeed, const float yawspeed) const
AngularVelocityBody TelemetryServiceImplTest::createAngularVelocityBody(
const float roll_rad_s, const float pitch_rad_s, const float yaw_rad_s) const
{
mavsdk::Telemetry::AngularSpeed angular_speed;
mavsdk::Telemetry::AngularVelocityBody angular_velocity_body;

angular_speed.rollspeed = rollspeed;
angular_speed.pitchspeed = pitchspeed;
angular_speed.yawspeed = yawspeed;
angular_velocity_body.roll_rad_s = roll_rad_s;
angular_velocity_body.pitch_rad_s = pitch_rad_s;
angular_velocity_body.yaw_rad_s = yaw_rad_s;

return angular_speed;
return angular_velocity_body;
}

void TelemetryServiceImplTest::checkSendsAttitudeQuaternions(
Expand All @@ -1136,27 +1139,29 @@ void TelemetryServiceImplTest::checkSendsAttitudeQuaternions(
}
}

void TelemetryServiceImplTest::checkSendsAttitudeAngularSpeeds(
const std::vector<AngularSpeed>& angular_speeds) const
void TelemetryServiceImplTest::checkSendsAttitudeAngularVelocitiesBody(
const std::vector<AngularVelocityBody>& angular_velocities_body) const
{
std::promise<void> subscription_promise;
auto subscription_future = subscription_promise.get_future();
mavsdk::Telemetry::attitude_angular_speed_callback_t attitude_angular_speed_callback;
EXPECT_CALL(*_telemetry, attitude_angular_speed_async(_))
.WillOnce(SaveCallback(&attitude_angular_speed_callback, &subscription_promise));

std::vector<AngularSpeed> received_angular_speeds;
auto angular_speed_stream_future = subscribeAttitudeAngularSpeedAsync(received_angular_speeds);
mavsdk::Telemetry::attitude_angular_velocity_body_callback_t
attitude_angular_velocity_body_callback;
EXPECT_CALL(*_telemetry, attitude_angular_velocity_body_async(_))
.WillOnce(SaveCallback(&attitude_angular_velocity_body_callback, &subscription_promise));

std::vector<AngularVelocityBody> received_angular_velocities_body;
auto angular_velocity_body_stream_future =
subscribeAttitudeAngularVelocityBodyAsync(received_angular_velocities_body);
subscription_future.wait();
for (const auto angular_speed : angular_speeds) {
attitude_angular_speed_callback(angular_speed);
for (const auto angular_velocity_body : angular_velocities_body) {
attitude_angular_velocity_body_callback(angular_velocity_body);
}
_telemetry_service->stop();
angular_speed_stream_future.wait();
angular_velocity_body_stream_future.wait();

ASSERT_EQ(angular_speeds.size(), received_angular_speeds.size());
for (size_t i = 0; i < angular_speeds.size(); i++) {
EXPECT_EQ(angular_speeds.at(i), received_angular_speeds.at(i));
ASSERT_EQ(angular_velocities_body.size(), received_angular_velocities_body.size());
for (size_t i = 0; i < angular_velocities_body.size(); i++) {
EXPECT_EQ(angular_velocities_body.at(i), received_angular_velocities_body.at(i));
}
}

Expand All @@ -1170,14 +1175,14 @@ TEST_F(TelemetryServiceImplTest, sendsMultipleAttitudeQuaternions)
checkSendsAttitudeQuaternions(quaternions);
}

TEST_F(TelemetryServiceImplTest, sendsMultipleAttitudeAngularSpeeds)
TEST_F(TelemetryServiceImplTest, sendsMultipleAttitudeAngularVelocityBodys)
{
std::vector<AngularSpeed> angular_speed;
angular_speed.push_back(createAngularSpeed(0.1f, 0.2f, 0.3f));
angular_speed.push_back(createAngularSpeed(2.1f, 0.4f, -2.2f));
angular_speed.push_back(createAngularSpeed(5.2f, 5.9f, 1.1f));
std::vector<AngularVelocityBody> angular_velocities_body;
angular_velocities_body.push_back(createAngularVelocityBody(0.1f, 0.2f, 0.3f));
angular_velocities_body.push_back(createAngularVelocityBody(2.1f, 0.4f, -2.2f));
angular_velocities_body.push_back(createAngularVelocityBody(5.2f, 5.9f, 1.1f));

checkSendsAttitudeAngularSpeeds(angular_speed);
checkSendsAttitudeAngularVelocitiesBody(angular_velocities_body);
}

TEST_F(TelemetryServiceImplTest, registersToTelemetryAttitudeEulerAsync)
Expand Down
17 changes: 9 additions & 8 deletions src/integration_tests/telemetry_async.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ static void print_in_air(bool in_air);
static void print_armed(bool armed);
static void print_quaternion(Telemetry::Quaternion quaternion);
static void print_euler_angle(Telemetry::EulerAngle euler_angle);
static void print_angular_speed(Telemetry::AngularSpeed angular_speed);
static void print_angular_velocity_body(Telemetry::AngularVelocityBody angular_velocity_body);
#if CAMERA_AVAILABLE == 1
static void print_camera_quaternion(Telemetry::Quaternion quaternion);
static void print_camera_euler_angle(Telemetry::EulerAngle euler_angle);
Expand All @@ -39,7 +39,7 @@ static bool _received_in_air = false;
static bool _received_armed = false;
static bool _received_quaternion = false;
static bool _received_euler_angle = false;
static bool _received_angular_speed = false;
static bool _received_angular_velocity_body = false;
#if CAMERA_AVAILABLE == 1
static bool _received_camera_quaternion = false;
static bool _received_camera_euler_angle = false;
Expand Down Expand Up @@ -116,7 +116,7 @@ TEST_F(SitlTest, TelemetryAsync)

telemetry->attitude_euler_angle_async(std::bind(&print_euler_angle, _1));

telemetry->attitude_angular_speed_async(std::bind(&print_angular_speed, _1));
telemetry->attitude_angular_velocity_body_async(std::bind(&print_angular_velocity_body, _1));

#if CAMERA_AVAILABLE == 1
telemetry->camera_attitude_quaternion_async(std::bind(&print_camera_quaternion, _1));
Expand Down Expand Up @@ -150,7 +150,7 @@ TEST_F(SitlTest, TelemetryAsync)
EXPECT_TRUE(_received_in_air);
EXPECT_TRUE(_received_armed);
EXPECT_TRUE(_received_quaternion);
EXPECT_TRUE(_received_angular_speed);
EXPECT_TRUE(_received_angular_velocity_body);
EXPECT_TRUE(_received_euler_angle);
#if CAMERA_AVAILABLE == 1
EXPECT_TRUE(_received_camera_quaternion);
Expand Down Expand Up @@ -219,12 +219,13 @@ void print_euler_angle(Telemetry::EulerAngle euler_angle)
_received_euler_angle = true;
}

void print_angular_speed(Telemetry::AngularSpeed angular_speed)
void print_angular_velocity_body(Telemetry::AngularVelocityBody angular_velocity_body)
{
std::cout << "Angular speed: [ " << angular_speed.rollspeed << ", " << angular_speed.pitchspeed
<< ", " << angular_speed.yawspeed << " ] rad/s" << std::endl;
std::cout << "Angular velocity: [ " << angular_velocity_body.roll_rad_s << ", "
<< angular_velocity_body.pitch_rad_s << ", " << angular_velocity_body.yaw_rad_s
<< " ] rad/s" << std::endl;

_received_angular_speed = true;
_received_angular_velocity_body = true;
}

#if CAMERA_AVAILABLE == 1
Expand Down
8 changes: 5 additions & 3 deletions src/integration_tests/telemetry_sync.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,11 @@ TEST_F(SitlTest, TelemetrySync)
std::cout << "Euler: (" << euler_angle.roll_deg << " deg, " << euler_angle.pitch_deg
<< " deg," << euler_angle.yaw_deg << " deg)" << std::endl;

const Telemetry::AngularSpeed& angular_speed = telemetry->attitude_angular_speed();
std::cout << "Angular speed: (" << angular_speed.rollspeed << ", "
<< angular_speed.pitchspeed << "," << angular_speed.yawspeed << ")" << std::endl;
const Telemetry::AngularVelocityBody& angular_velocity_body =
telemetry->attitude_angular_velocity_body();
std::cout << "Angular velocity: (" << angular_velocity_body.roll_rad_s << ", "
<< angular_velocity_body.pitch_rad_s << "," << angular_velocity_body.yaw_rad_s
<< ")" << std::endl;

const Telemetry::GroundSpeedNED& ground_speed_ned = telemetry->ground_speed_ned();
std::cout << "Speed: (" << ground_speed_ned.velocity_north_m_s << " m/s, "
Expand Down
Loading

0 comments on commit 6351b11

Please sign in to comment.