mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 04:53:35 +08:00
mavlink: move EXTENDED_SYS_STATE stream to separate file
This commit is contained in:
Daniel Agar
@@ -58,8 +58,8 @@ px4_add_module(
|
||||
mavlink_shell.cpp
|
||||
mavlink_simple_analyzer.cpp
|
||||
mavlink_stream.cpp
|
||||
mavlink_ulog.cpp
|
||||
mavlink_timesync.cpp
|
||||
mavlink_ulog.cpp
|
||||
tune_publisher.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
|
||||
@@ -45,11 +45,6 @@
|
||||
#include "mavlink_simple_analyzer.h"
|
||||
#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"
|
||||
|
||||
#include <commander/px4_custom_mode.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
@@ -123,6 +118,12 @@
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_2pi;
|
||||
|
||||
#include "streams/autopilot_version.h"
|
||||
#include "streams/flight_information.h"
|
||||
#include "streams/protocol_version.h"
|
||||
#include "streams/storage_information.h"
|
||||
#include "streams/EXTENDED_SYS_STATE.hpp"
|
||||
|
||||
// ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
|
||||
static_assert(MAV_SENSOR_ROTATION_NONE == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_NONE),
|
||||
"Roll: 0, Pitch: 0, Yaw: 0");
|
||||
@@ -4665,118 +4666,6 @@ protected:
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamExtendedSysState : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const override
|
||||
{
|
||||
return MavlinkStreamExtendedSysState::get_name_static();
|
||||
}
|
||||
|
||||
static constexpr const char *get_name_static()
|
||||
{
|
||||
return "EXTENDED_SYS_STATE";
|
||||
}
|
||||
|
||||
static constexpr uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
|
||||
}
|
||||
|
||||
uint16_t get_id() override
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamExtendedSysState(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
private:
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
mavlink_extended_sys_state_t _msg;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &) = delete;
|
||||
MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &) = delete;
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_msg()
|
||||
{
|
||||
_msg.vtol_state = MAV_VTOL_STATE_UNDEFINED;
|
||||
_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
|
||||
}
|
||||
|
||||
bool send(const hrt_abstime t) override
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
vehicle_status_s status;
|
||||
|
||||
if (_status_sub.copy(&status)) {
|
||||
updated = true;
|
||||
|
||||
if (status.is_vtol) {
|
||||
if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_MC;
|
||||
|
||||
} else if (!status.in_transition_mode) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_FW;
|
||||
|
||||
} else if (status.in_transition_mode && status.in_transition_to_fw) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW;
|
||||
|
||||
} else if (status.in_transition_mode) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vehicle_land_detected_s land_detected;
|
||||
|
||||
if (_landed_sub.copy(&land_detected)) {
|
||||
updated = true;
|
||||
|
||||
if (land_detected.landed) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
|
||||
|
||||
} else if (!land_detected.landed) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_IN_AIR;
|
||||
|
||||
vehicle_control_mode_s control_mode;
|
||||
position_setpoint_triplet_s pos_sp_triplet;
|
||||
|
||||
if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) {
|
||||
if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
|
||||
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_TAKEOFF;
|
||||
|
||||
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_LANDING;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (updated) {
|
||||
mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamAltitude : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
@@ -5627,7 +5516,9 @@ static const StreamListItem streams_list[] = {
|
||||
create_stream_list_item<MavlinkStreamCameraTrigger>(),
|
||||
create_stream_list_item<MavlinkStreamCameraImageCaptured>(),
|
||||
create_stream_list_item<MavlinkStreamDistanceSensor>(),
|
||||
#ifdef EXTENDED_SYS_STATE_HPP
|
||||
create_stream_list_item<MavlinkStreamExtendedSysState>(),
|
||||
#endif // EXTENDED_SYS_STATE_HPP
|
||||
create_stream_list_item<MavlinkStreamAltitude>(),
|
||||
create_stream_list_item<MavlinkStreamADSBVehicle>(),
|
||||
create_stream_list_item<MavlinkStreamUTMGlobalPosition>(),
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef EXTENDED_SYS_STATE_HPP
|
||||
#define EXTENDED_SYS_STATE_HPP
|
||||
|
||||
class MavlinkStreamExtendedSysState : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const override
|
||||
{
|
||||
return MavlinkStreamExtendedSysState::get_name_static();
|
||||
}
|
||||
|
||||
static constexpr const char *get_name_static()
|
||||
{
|
||||
return "EXTENDED_SYS_STATE";
|
||||
}
|
||||
|
||||
static constexpr uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
|
||||
}
|
||||
|
||||
uint16_t get_id() override
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamExtendedSysState(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
private:
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
mavlink_extended_sys_state_t _msg;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &) = delete;
|
||||
MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &) = delete;
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_msg()
|
||||
{
|
||||
_msg.vtol_state = MAV_VTOL_STATE_UNDEFINED;
|
||||
_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
|
||||
}
|
||||
|
||||
bool send(const hrt_abstime t) override
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
vehicle_status_s status;
|
||||
|
||||
if (_status_sub.copy(&status)) {
|
||||
updated = true;
|
||||
|
||||
if (status.is_vtol) {
|
||||
if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_MC;
|
||||
|
||||
} else if (!status.in_transition_mode) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_FW;
|
||||
|
||||
} else if (status.in_transition_mode && status.in_transition_to_fw) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW;
|
||||
|
||||
} else if (status.in_transition_mode) {
|
||||
_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vehicle_land_detected_s land_detected;
|
||||
|
||||
if (_landed_sub.copy(&land_detected)) {
|
||||
updated = true;
|
||||
|
||||
if (land_detected.landed) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
|
||||
|
||||
} else if (!land_detected.landed) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_IN_AIR;
|
||||
|
||||
vehicle_control_mode_s control_mode;
|
||||
position_setpoint_triplet_s pos_sp_triplet;
|
||||
|
||||
if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) {
|
||||
if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
|
||||
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_TAKEOFF;
|
||||
|
||||
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
_msg.landed_state = MAV_LANDED_STATE_LANDING;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (updated) {
|
||||
mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // EXTENDED_SYS_STATE_HPP
|
||||
Reference in New Issue
Block a user