diff --git a/src/integration_tests/telemetry_sync.cpp b/src/integration_tests/telemetry_sync.cpp index ba74bd209b..2e1cbfb9c8 100644 --- a/src/integration_tests/telemetry_sync.cpp +++ b/src/integration_tests/telemetry_sync.cpp @@ -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(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)); } } diff --git a/src/plugins/telemetry/include/plugins/telemetry/telemetry.h b/src/plugins/telemetry/include/plugins/telemetry/telemetry.h index 76b8b77daf..fdae22116f 100644 --- a/src/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/src/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -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 diff --git a/src/plugins/telemetry/telemetry.cpp b/src/plugins/telemetry/telemetry.cpp index dd364277d9..0025ca181d 100644 --- a/src/plugins/telemetry/telemetry.cpp +++ b/src/plugins/telemetry/telemetry.cpp @@ -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