diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a683d078b8..010ac898fb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -46,6 +46,7 @@ #include "mavlink_high_latency2.h" #include "streams/autopilot_version.h" +#include "streams/flight_information.h" #include "streams/protocol_version.h" #include "streams/storage_information.h" diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bd9fefff9f..2fbd96e51c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -346,23 +346,6 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c return target_ok; } -void -MavlinkReceiver::send_flight_information() -{ - mavlink_flight_information_t flight_info{}; - flight_info.flight_uuid = static_cast(_param_com_flight_uuid.get()); - - actuator_armed_s actuator_armed{}; - bool ret = _actuator_armed_sub.copy(&actuator_armed); - - if (ret && actuator_armed.timestamp != 0) { - flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms; - } - - flight_info.time_boot_ms = hrt_absolute_time() / 1000; - mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info); -} - void MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) { @@ -446,9 +429,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)roundf(cmd_mavlink.param1)); - } else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) { - send_flight_information(); - } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { uint16_t message_id = (uint16_t)roundf(cmd_mavlink.param1); bool stream_found = false; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 476a93cd3a..475800aee8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -204,9 +204,6 @@ private: bool evaluate_target_ok(int command, int target_system, int target_component); - void send_flight_information(); - void send_storage_information(int storage_id); - void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust); void schedule_tune(const char *tune); diff --git a/src/modules/mavlink/streams/flight_information.h b/src/modules/mavlink/streams/flight_information.h new file mode 100644 index 0000000000..824db1c93a --- /dev/null +++ b/src/modules/mavlink/streams/flight_information.h @@ -0,0 +1,77 @@ +#ifndef MAVLINK_STREAM_FLIGHT_INFORMATION_H +#define MAVLINK_STREAM_FLIGHT_INFORMATION_H + +#include "../mavlink_messages.h" + +#include + +class MavlinkStreamFlightInformation : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamFlightInformation::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "FLIGHT_INFORMATION"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_FLIGHT_INFORMATION; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamFlightInformation(mavlink); + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool request_message(float param1, float param2, float param3, float param4, + float param5, float param6, float param7) override + { + return send(hrt_absolute_time()); + } +private: + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + + /* do not allow top copying this class */ + MavlinkStreamFlightInformation(MavlinkStreamFlightInformation &) = delete; + MavlinkStreamFlightInformation &operator = (const MavlinkStreamFlightInformation &) = delete; +protected: + explicit MavlinkStreamFlightInformation(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + actuator_armed_s actuator_armed{}; + bool ret = _armed_sub.copy(&actuator_armed); + + if (ret && actuator_armed.timestamp != 0) { + const param_t param_com_flight_uuid = param_find("COM_FLIGHT_UUID"); + int32_t flight_uuid; + param_get(param_com_flight_uuid, &flight_uuid); + + mavlink_flight_information_t flight_info{}; + flight_info.flight_uuid = static_cast(flight_uuid); + flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms; + flight_info.time_boot_ms = hrt_absolute_time() / 1000; + mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info); + } + + return ret; + } +}; + +#endif