MAVLink: Code style

This commit is contained in:
Lorenz Meier
2016-08-26 23:43:33 +02:00
parent e67a6bdfc2
commit 720c445f21
17 changed files with 922 additions and 627 deletions
+26 -12
View File
@@ -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);
}
}