Daniel Agar
4 years ago
3 changed files with 158 additions and 118 deletions
@ -0,0 +1,149 @@
@@ -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
|
||||
Loading…
Reference in new issue