diff options
Diffstat (limited to 'src/modules/mavlink/mavlink.c')
| -rw-r--r-- | src/modules/mavlink/mavlink.c | 29 |
1 files changed, 24 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c10e297b..4c38cf35a 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,12 +68,13 @@ #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "util.h" #include "waypoints.h" #include "mavlink_parameters.h" +#include <uORB/topics/mission_result.h> + /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); @@ -219,6 +220,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO use control_mode topic + /* if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { @@ -232,6 +235,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } + */ } *mavlink_custom_mode = custom_mode.data; @@ -644,6 +648,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + thread_running = true; /* arm counter to go off immediately */ @@ -690,25 +698,36 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); + } + } + + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); if (baudrate > 57600) { mavlink_pm_queued_send(); |