|
|
|
@ -81,7 +81,6 @@
@@ -81,7 +81,6 @@
|
|
|
|
|
#include <cstring> |
|
|
|
|
|
|
|
|
|
#include <uORB/topics/mavlink_log.h> |
|
|
|
|
#include <uORB/topics/mission.h> |
|
|
|
|
|
|
|
|
|
typedef enum VEHICLE_MODE_FLAG { |
|
|
|
|
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ |
|
|
|
@ -3500,7 +3499,7 @@ void Commander::enable_hil()
@@ -3500,7 +3499,7 @@ void Commander::enable_hil()
|
|
|
|
|
void Commander::mission_init() |
|
|
|
|
{ |
|
|
|
|
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ |
|
|
|
|
mission_s mission{}; |
|
|
|
|
mission_s mission; |
|
|
|
|
|
|
|
|
|
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { |
|
|
|
|
if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { |
|
|
|
@ -3517,8 +3516,7 @@ void Commander::mission_init()
@@ -3517,8 +3516,7 @@ void Commander::mission_init()
|
|
|
|
|
dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uORB::Publication<mission_s> mission_pub{ORB_ID(mission)}; |
|
|
|
|
mission_pub.publish(mission); |
|
|
|
|
_mission_pub.publish(mission); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|