fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 14:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mission merge offboard + onboard and simplify

Browse Source
This commit is contained in:
Daniel Agar
2017-11-14 16:36:11 -05:00
committed by Lorenz Meier
parent 956935141e
commit 916d6a15fd
14 changed files with 181 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+10 -15
View File
@@ -1414,33 +1414,28 @@ Commander::run()
/* command ack */
orb_advert_t command_ack_pub = nullptr;
orb_advert_t commander_state_pub = nullptr;
orb_advert_t vehicle_status_flags_pub = nullptr;
vehicle_status_flags_s vehicle_status_flags = {};
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
mission_s mission;
orb_advert_t commander_state_pub = nullptr;
vehicle_status_flags_s vehicle_status_flags = {};
orb_advert_t vehicle_status_flags_pub = nullptr;
mission_s mission = {};
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
if (mission.count > 0) {
mavlink_log_info(&mavlink_log_pub, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
mission.dataman_id, mission.count, mission.current_seq);
PX4_INFO("Mission #%d loaded, %u WPs, curr: %d", mission.dataman_id, mission.count, mission.current_seq);
}
} else {
mavlink_log_critical(&mavlink_log_pub, "reading mission state failed");
PX4_ERR("reading mission state failed");
/* initialize mission state in dataman */
mission.dataman_id = 0;
mission.count = 0;
mission.current_seq = 0;
mission.timestamp = hrt_absolute_time();
mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
}
orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
orb_advert_t mission_pub = orb_advertise(ORB_ID(mission), &mission);
orb_unadvertise(mission_pub);
}