From 220011914c01ef4050ca487059b0d317e6b53fb7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 21:57:01 +0400 Subject: navigator: AUTO_READY nav state added, RTL implemented properly --- src/modules/mavlink/mavlink.c | 4 +++- src/modules/mavlink/waypoints.c | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d4e4c027b..4d3c9dd2c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,7 +220,9 @@ 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; - if (control_mode.nav_state == NAV_STATE_NONE) { + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 1adfeafde..168666d4e 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -152,7 +152,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ + mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -184,7 +184,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; -- cgit v1.2.3