dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: set current WP working as expected, report current WP with 0.5 Hz

This commit is contained in:
Julian Oes 2014-02-24 15:17:13 +01:00
parent 9c5894239f
commit 926c4701c7
4 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mavlink/mavlink_main.cpp
View File

@ -987,8 +987,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
} else if (seq == 0 && wpm->size == 0) {
/* don't broadcast if no WPs */
@ -1145,7 +1143,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mission.current_index = wpc.seq;
publish_mission();
mavlink_wpm_send_waypoint_current(wpc.seq);
/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
@ -1708,8 +1707,7 @@ Mavlink::task_main(int argc, char *argv[])
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
unsigned lowspeed_counter = 0;
/* wakeup source(s) */
struct pollfd fds[1];
@ -1739,7 +1737,7 @@ Mavlink::task_main(int argc, char *argv[])
}
/* 1 Hz */
if (lowspeed_counter == 10) {
if (lowspeed_counter % 10 == 0) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
@ -1772,7 +1770,12 @@ Mavlink::task_main(int argc, char *argv[])
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
/* 0.5 Hz */
if (lowspeed_counter % 20 == 0) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
lowspeed_counter++;
@ -1783,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (_verbose) warnx("Got mission result");
if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission);
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);

2
src/modules/navigator/navigator_main.cpp
View File

@ -1105,7 +1105,7 @@ Navigator::set_mission_item()
if (ret == OK) {
_mission.report_current_mission_item();
_mission.report_current_offboard_mission_item();
/* reset time counter for new item */
_time_first_inside_orbit = 0;

8
src/modules/navigator/navigator_mission.cpp
View File

@ -89,7 +89,7 @@ Mission::set_current_offboard_mission_index(int new_index)
_current_offboard_mission_index = 0;
}
}
report_current_mission_item();
report_current_offboard_mission_item();
}
void
@ -296,11 +296,9 @@ Mission::report_mission_item_reached()
}
void
Mission::report_current_mission_item()
Mission::report_current_offboard_mission_item()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.index_current_mission = _current_offboard_mission_index;
}
_mission_result.index_current_mission = _current_offboard_mission_index;
}
void

2
src/modules/navigator/navigator_mission.h
View File

@ -73,7 +73,7 @@ public:
void move_to_next();
void report_mission_item_reached();
void report_current_mission_item();
void report_current_offboard_mission_item();
void publish_mission_result();
private: