mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-17 03:01:36 +08:00
MAVLink: Code style
This commit is contained in:
@@ -66,9 +66,9 @@ int MavlinkMissionManager::_current_seq = 0;
|
||||
bool MavlinkMissionManager::_transfer_in_progress = false;
|
||||
|
||||
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
|
||||
((_msg.target_component == mavlink_system.compid) || \
|
||||
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
|
||||
(_msg.target_component == MAV_COMP_ID_ALL)))
|
||||
((_msg.target_component == mavlink_system.compid) || \
|
||||
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
|
||||
(_msg.target_component == MAV_COMP_ID_ALL)))
|
||||
|
||||
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_state(MAVLINK_WPM_STATE_IDLE),
|
||||
@@ -120,9 +120,11 @@ void
|
||||
MavlinkMissionManager::init_offboard_mission()
|
||||
{
|
||||
mission_s mission_state;
|
||||
|
||||
if (!_dataman_init) {
|
||||
_dataman_init = true;
|
||||
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s);
|
||||
|
||||
if (ret > 0) {
|
||||
_dataman_id = mission_state.dataman_id;
|
||||
_count = mission_state.count;
|
||||
@@ -132,6 +134,7 @@ MavlinkMissionManager::init_offboard_mission()
|
||||
_dataman_id = 0;
|
||||
_count = 0;
|
||||
_current_seq = 0;
|
||||
|
||||
} else {
|
||||
PX4_WARN("offboard mission init failed");
|
||||
_dataman_id = 0;
|
||||
@@ -139,6 +142,7 @@ MavlinkMissionManager::init_offboard_mission()
|
||||
_current_seq = 0;
|
||||
}
|
||||
}
|
||||
|
||||
_my_dataman_id = _dataman_id;
|
||||
}
|
||||
|
||||
@@ -176,6 +180,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
|
||||
|
||||
} else {
|
||||
warnx("WPM: ERROR: can't save mission state");
|
||||
|
||||
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
|
||||
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
|
||||
}
|
||||
@@ -261,6 +266,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
||||
|
||||
} else {
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||
|
||||
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
|
||||
_mavlink->send_statustext_critical("Mission storage: Unable to read from microSD");
|
||||
}
|
||||
@@ -588,11 +594,11 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
||||
if (_state == MAVLINK_WPM_STATE_IDLE) {
|
||||
_time_last_recv = hrt_absolute_time();
|
||||
|
||||
if(_transfer_in_progress)
|
||||
{
|
||||
if (_transfer_in_progress) {
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||
return;
|
||||
}
|
||||
|
||||
_transfer_in_progress = true;
|
||||
|
||||
if (wpc.count > _max_count) {
|
||||
@@ -683,6 +689,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
struct mission_item_s mission_item = {};
|
||||
|
||||
int ret = parse_mavlink_mission_item(&wp, &mission_item);
|
||||
|
||||
if (ret != OK) {
|
||||
@@ -698,7 +705,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
|
||||
|
||||
dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
|
||||
|
||||
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
|
||||
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item,
|
||||
sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
|
||||
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
|
||||
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||
@@ -729,6 +737,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
|
||||
} else {
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||
}
|
||||
|
||||
_transfer_in_progress = false;
|
||||
|
||||
} else {
|
||||
@@ -753,6 +762,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
||||
|
||||
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
|
||||
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
|
||||
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
|
||||
|
||||
} else {
|
||||
@@ -768,10 +778,11 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
int
|
||||
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
|
||||
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
|
||||
struct mission_item_s *mission_item)
|
||||
{
|
||||
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
|
||||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
|
||||
// only support global waypoints for now
|
||||
|
||||
@@ -781,6 +792,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
||||
|
||||
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
|
||||
mission_item->altitude_is_relative = false;
|
||||
|
||||
} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
mission_item->altitude_is_relative = true;
|
||||
}
|
||||
@@ -897,7 +909,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
||||
|
||||
|
||||
int
|
||||
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
|
||||
mavlink_mission_item_t *mavlink_mission_item)
|
||||
{
|
||||
mavlink_mission_item->frame = mission_item->frame;
|
||||
mavlink_mission_item->command = mission_item->nav_cmd;
|
||||
@@ -943,6 +956,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
||||
|
||||
if (mission_item->altitude_is_relative) {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
|
||||
} else {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
}
|
||||
@@ -997,10 +1011,10 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
||||
|
||||
void MavlinkMissionManager::check_active_mission(void)
|
||||
{
|
||||
if(!(_my_dataman_id==_dataman_id))
|
||||
{
|
||||
if (!(_my_dataman_id == _dataman_id)) {
|
||||
if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
|
||||
_my_dataman_id=_dataman_id;
|
||||
|
||||
_my_dataman_id = _dataman_id;
|
||||
this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user