Skip to content

Commit

Permalink
Small improvements to telemetry plugin and test (#863)
Browse files Browse the repository at this point in the history 
* telemetry: add two missing stream operators

* integration_tests: simplify sync telemetry test
  • Loading branch information
@julianoes
julianoes committed Sep 26, 2019
1 parent b9bd5fe commit 9cea5a4
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 83 deletions.
111 changes: 28 additions & 83 deletions src/integration_tests/telemetry_sync.cpp
View file
Expand Up @@ -7,96 +7,41 @@ using namespace mavsdk;

TEST_F(SitlTest, TelemetrySync)
{
Mavsdk dc;
Mavsdk mavsdk;

ConnectionResult ret = dc.add_udp_connection();
ConnectionResult ret = mavsdk.add_udp_connection();
ASSERT_EQ(ret, ConnectionResult::SUCCESS);

std::this_thread::sleep_for(std::chrono::seconds(2));
System& system = dc.system();
System& system = mavsdk.system();
ASSERT_TRUE(system.is_connected());

auto telemetry = std::make_shared<Telemetry>(system);

while (!telemetry->health_all_ok()) {
std::cout << "waiting for system to be ready" << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
}

Telemetry::Result result = telemetry->set_rate_position(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_home_position(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_in_air(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_attitude(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_ground_speed_ned(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_gps_info(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_battery(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);
result = telemetry->set_rate_actuator_control_target(10.0);
ASSERT_EQ(result, Telemetry::Result::SUCCESS);

// Print 3s of telemetry.
for (unsigned i = 0; i < 50; ++i) {
const Telemetry::Position& position = telemetry->position();
std::cout << "Position: " << std::endl
<< "Absolute altitude: " << position.absolute_altitude_m << " m" << std::endl
<< "Relative altitude: " << position.relative_altitude_m << " m" << std::endl
<< "Latitude: " << position.latitude_deg << " deg"
<< ", longitude: " << position.longitude_deg << " deg" << std::endl;

const Telemetry::Position& home_position = telemetry->home_position();
std::cout << "Home position: " << std::endl
<< "Absolute altitude: " << home_position.absolute_altitude_m << " m" << std::endl
<< "Relative altitude: " << home_position.relative_altitude_m << " m" << std::endl
<< "Latitude: " << home_position.latitude_deg << " deg"
<< ", longitude: " << home_position.longitude_deg << " deg" << std::endl;

std::cout << (telemetry->in_air() ? "In-air" : "On-ground") << std::endl;

const Telemetry::Quaternion& quaternion = telemetry->attitude_quaternion();
std::cout << "Quaternion: (" << quaternion.w << ", " << quaternion.x << "," << quaternion.y
<< "," << quaternion.z << ")" << std::endl;

const Telemetry::EulerAngle& euler_angle = telemetry->attitude_euler_angle();
std::cout << "Euler: (" << euler_angle.roll_deg << " deg, " << euler_angle.pitch_deg
<< " deg," << euler_angle.yaw_deg << " deg)" << 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, "
<< ground_speed_ned.velocity_east_m_s << " m/s,"
<< ground_speed_ned.velocity_down_m_s << " m/s)" << std::endl;

const Telemetry::GPSInfo& gps_info = telemetry->gps_info();
std::cout << "GPS sats: " << gps_info.num_satellites << ", fix type: " << gps_info.fix_type
EXPECT_EQ(telemetry->set_rate_position(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_home_position(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_in_air(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_attitude(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_ground_speed_ned(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_gps_info(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_battery(10.0), Telemetry::Result::SUCCESS);
EXPECT_EQ(telemetry->set_rate_actuator_control_target(10.0), Telemetry::Result::SUCCESS);

for (unsigned i = 0; i < 10; ++i) {
std::cout << "Position: " << telemetry->position() << std::endl;
std::cout << "Home Position: " << telemetry->home_position() << std::endl;
std::cout << "Attitude: " << telemetry->attitude_quaternion() << std::endl;
std::cout << "Attitude: " << telemetry->attitude_euler_angle() << std::endl;
std::cout << "Angular velocity: " << telemetry->attitude_angular_velocity_body()
<< std::endl;

const Telemetry::Battery& battery = telemetry->battery();
std::cout << "Battery voltage: " << battery.voltage_v << " v, "
<< "remaining: " << battery.remaining_percent * 100.0f << " %" << std::endl;

const Telemetry::ActuatorControlTarget& actuator_control_target =
telemetry->actuator_control_target();
std::cout << "Actuator control target: group: " << actuator_control_target.group
<< ", controls: [";
for (int k = 0; k < 8; k++) {
std::cout << actuator_control_target.controls[i];
if (k < 7) {
std::cout << ", ";
} else {
std::cout << "]" << std::endl;
}
}

std::this_thread::sleep_for(std::chrono::milliseconds(30));
std::cout << "Ground speed: " << telemetry->ground_speed_ned() << std::endl;
std::cout << "GPS Info: " << telemetry->gps_info() << std::endl;
std::cout << "Battery: " << telemetry->battery() << std::endl;
std::cout << "Actuators: " << telemetry->actuator_control_target() << std::endl;
std::cout << "Flight mode: " << telemetry->flight_mode() << std::endl;
std::cout << "Landed state: " << telemetry->landed_state()
<< "(in air: " << telemetry->in_air() << ")" << std::endl;

std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
14 changes: 14 additions & 0 deletions src/plugins/telemetry/include/plugins/telemetry/telemetry.h
View file
Expand Up @@ -1308,4 +1308,18 @@ bool operator==(
std::ostream&
operator<<(std::ostream& str, Telemetry::ActuatorOutputStatus const& actuator_output_status);

/**
* @brief Stream operator to print information about a `Telemetry::FlightMode`.
*
* @returns A reference to the stream.
*/
std::ostream& operator<<(std::ostream& str, Telemetry::FlightMode const& flight_mode);

/**
* @brief Stream operator to print information about a `Telemetry::LandedState`.
*
* @returns A reference to the stream.
*/
std::ostream& operator<<(std::ostream& str, Telemetry::LandedState const& landed_state);

} // namespace mavsdk
10 changes: 10 additions & 0 deletions src/plugins/telemetry/telemetry.cpp
View file
Expand Up @@ -730,4 +730,14 @@ operator<<(std::ostream& str, Telemetry::ActuatorOutputStatus const& actuator_ou
return str;
}

std::ostream& operator<<(std::ostream& str, Telemetry::FlightMode const& flight_mode)
{
return str << Telemetry::flight_mode_str(flight_mode);
}

std::ostream& operator<<(std::ostream& str, Telemetry::LandedState const& landed_state)
{
return str << Telemetry::landed_state_str(landed_state);
}

} // namespace mavsdk

0 comments on commit 9cea5a4

Please sign in to comment.