pixhawk commander--navigator--modules之间的联系

commander--navigator是决策部分，处理得到飞行模式和期望导航路线。此blog只是记下commander--navigator--modules之间的联系，从决策部分如何影响控制部分，进而完成任务，此blog不涉及详细的运行细节。

1.commander.cpp中通过将遥控信息处理发布orb_publish(ORB_ID(vehicle_status), status_pub, &status);

处理过程参考pixhawk _control_mode如何产生的

2.navigator_main.cpp中

[cpp] view plain copy print?

1. void  
2. Navigator::vehicle_status_update()  
3. {  
4.     if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {  
5.         /* in case the commander is not be running */  
6.         _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;  
7.     }  
8. }  

voidNavigator::vehicle_status_update(){if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {/* in case the commander is not be running */_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;}}

获取commander.cpp发布的ORB_ID(vehicle_status)主题

[cpp] view plain copy print?

1. /* Do stuff according to navigation state set by commander */  
2. switch (_vstatus.nav_state) {  
3.     case vehicle_status_s::NAVIGATION_STATE_MANUAL:  
4.     case vehicle_status_s::NAVIGATION_STATE_ACRO:  
5.     case vehicle_status_s::NAVIGATION_STATE_ALTCTL:  
6.     case vehicle_status_s::NAVIGATION_STATE_POSCTL:  
7.     case vehicle_status_s::NAVIGATION_STATE_TERMINATION:  
8.     case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:  
9.         _navigation_mode = nullptr;  
10.         _can_loiter_at_sp = false;  
11.         break;  
12.     case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:  
13.         if (_fw_pos_ctrl_status.abort_landing) {  
14.             // pos controller aborted landing, requests loiter  
15.             // above landing waypoint  
16.             _navigation_mode = &_loiter;  
17.             _pos_sp_triplet_published_invalid_once = false;  
18.         } else {  
19.             _pos_sp_triplet_published_invalid_once = false;  
20.             _navigation_mode = &_mission;  
21.         }  
22.         break;  
23.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:  
24.         _pos_sp_triplet_published_invalid_once = false;  
25.         _navigation_mode = &_loiter;  
26.         break;  
27.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:  
28.         _pos_sp_triplet_published_invalid_once = false;  
29.         if (_param_rcloss_act.get() == 1) {  
30.             _navigation_mode = &_loiter;  
31.         } else if (_param_rcloss_act.get() == 3) {  
32.             _navigation_mode = &_land;  
33.         } else if (_param_rcloss_act.get() == 4) {  
34.             _navigation_mode = &_rcLoss;  
35.         } else { /* if == 2 or unknown, RTL */  
36.             _navigation_mode = &_rtl;  
37.         }  
38.         break;  
39.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:  
40.         _pos_sp_triplet_published_invalid_once = false;  
41.         _navigation_mode = &_rtl;  
42.         break;  
43.     case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:  
44.         _pos_sp_triplet_published_invalid_once = false;  
45.         _navigation_mode = &_takeoff;  
46.         break;  
47.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:  
48.         _pos_sp_triplet_published_invalid_once = false;  
49.         _navigation_mode = &_land;  
50.         break;  
51.     case vehicle_status_s::NAVIGATION_STATE_DESCEND:  
52.         _pos_sp_triplet_published_invalid_once = false;  
53.         _navigation_mode = &_land;  
54.         break;  
55.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:  
56.         /* Use complex data link loss mode only when enabled via param 
57.         * otherwise use rtl */  
58.         _pos_sp_triplet_published_invalid_once = false;  
59.         if (_param_datalinkloss_act.get() == 1) {  
60.             _navigation_mode = &_loiter;  
61.         } else if (_param_datalinkloss_act.get() == 3) {  
62.             _navigation_mode = &_land;  
63.         } else if (_param_datalinkloss_act.get() == 4) {  
64.             _navigation_mode = &_dataLinkLoss;  
65.         } else { /* if == 2 or unknown, RTL */  
66.             _navigation_mode = &_rtl;  
67.         }  
68.         break;  
69.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:  
70.         _pos_sp_triplet_published_invalid_once = false;  
71.         _navigation_mode = &_engineFailure;  
72.         break;  
73.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:  
74.         _pos_sp_triplet_published_invalid_once = false;  
75.         _navigation_mode = &_gpsFailure;  
76.         break;  
77.     case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:  
78.         _pos_sp_triplet_published_invalid_once = false;  
79.         _navigation_mode = &_follow_target;  
80.         break;  
81.     default:  
82.         _navigation_mode = nullptr;  
83.         _can_loiter_at_sp = false;  
84.         break;  
85. }  
86.   
87. /* iterate through navigation modes and set active/inactive for each */  
88. for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {  
89.     _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);  
90. }  

/* Do stuff according to navigation state set by commander */switch (_vstatus.nav_state) {case vehicle_status_s::NAVIGATION_STATE_MANUAL:case vehicle_status_s::NAVIGATION_STATE_ACRO:case vehicle_status_s::NAVIGATION_STATE_ALTCTL:case vehicle_status_s::NAVIGATION_STATE_POSCTL:case vehicle_status_s::NAVIGATION_STATE_TERMINATION:case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:_navigation_mode = nullptr;_can_loiter_at_sp = false;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:if (_fw_pos_ctrl_status.abort_landing) {// pos controller aborted landing, requests loiter// above landing waypoint_navigation_mode = &_loiter;_pos_sp_triplet_published_invalid_once = false;} else {_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_mission;}break;case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_loiter;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:_pos_sp_triplet_published_invalid_once = false;if (_param_rcloss_act.get() == 1) {_navigation_mode = &_loiter;} else if (_param_rcloss_act.get() == 3) {_navigation_mode = &_land;} else if (_param_rcloss_act.get() == 4) {_navigation_mode = &_rcLoss;} else { /* if == 2 or unknown, RTL */_navigation_mode = &_rtl;}break;case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_rtl;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_takeoff;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_land;break;case vehicle_status_s::NAVIGATION_STATE_DESCEND:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_land;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:/* Use complex data link loss mode only when enabled via param* otherwise use rtl */_pos_sp_triplet_published_invalid_once = false;if (_param_datalinkloss_act.get() == 1) {_navigation_mode = &_loiter;} else if (_param_datalinkloss_act.get() == 3) {_navigation_mode = &_land;} else if (_param_datalinkloss_act.get() == 4) {_navigation_mode = &_dataLinkLoss;} else { /* if == 2 or unknown, RTL */_navigation_mode = &_rtl;}break;case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_engineFailure;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_gpsFailure;break;case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = &_follow_target;break;default:_navigation_mode = nullptr;_can_loiter_at_sp = false;break;}/* iterate through navigation modes and set active/inactive for each */for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);}

根据ORB_ID(vehicle_status)主题选择不同的导航模式

导航模式有10种

[cpp] view plain copy print?

1. /* Create a list of our possible navigation types */  
2. _navigation_mode_array[0] = &_mission;  
3. _navigation_mode_array[1] = &_loiter;  
4. _navigation_mode_array[2] = &_rtl;  
5. _navigation_mode_array[3] = &_dataLinkLoss;  
6. _navigation_mode_array[4] = &_engineFailure;  
7. _navigation_mode_array[5] = &_gpsFailure;  
8. _navigation_mode_array[6] = &_rcLoss;  
9. _navigation_mode_array[7] = &_takeoff;  
10. _navigation_mode_array[8] = &_land;  
11. _navigation_mode_array[9] = &_follow_target;  

/* Create a list of our possible navigation types */_navigation_mode_array[0] = &_mission;_navigation_mode_array[1] = &_loiter;_navigation_mode_array[2] = &_rtl;_navigation_mode_array[3] = &_dataLinkLoss;_navigation_mode_array[4] = &_engineFailure;_navigation_mode_array[5] = &_gpsFailure;_navigation_mode_array[6] = &_rcLoss;_navigation_mode_array[7] = &_takeoff;_navigation_mode_array[8] = &_land;_navigation_mode_array[9] = &_follow_target;

请注意

[cpp] view plain copy print?

1. for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {  
2.     _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);  
3. }  

for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);}

这里的run()函数里面是运行继承的函数，即on_activation();on_active();on_inactive();，因此会跳到相应的cpp文件中(因此navigator文件夹中的那么多cpp文件得以用上)

[cpp] view plain copy print?

1. void  
2. NavigatorMode::run(bool active)  
3. {  
4.     if (active) {  
5.         if (_first_run) {  
6.             /* first run */  
7.             _first_run = false;  
8.             /* Reset stay in failsafe flag */  
9.             _navigator->get_mission_result()->stay_in_failsafe = false;  
10.             _navigator->set_mission_result_updated();  
11.             on_activation();  
12.         } else {  
13.             /* periodic updates when active */  
14.             on_active();  
15.         }  
16.     } else {  
17.         /* periodic updates when inactive */  
18.         _first_run = true;  
19.         on_inactive();  
20.     }  
21. }  

voidNavigatorMode::run(bool active){if (active) {if (_first_run) {/* first run */_first_run = false;/* Reset stay in failsafe flag */_navigator->get_mission_result()->stay_in_failsafe = false;_navigator->set_mission_result_updated();on_activation();} else {/* periodic updates when active */on_active();}} else {/* periodic updates when inactive */_first_run = true;on_inactive();}}

那么以rtl.cpp(自动返航)为例，看看逻辑上如何完成这个自动返航这个任务的
由上程序可知，第一次运行on_activation();，以后就运行on_active();

[cpp] view plain copy print?

1. void  
2. RTL::on_activation()  
3. {  
4.     /* reset starting point so we override what the triplet contained from the previous navigation state */  
5.     _rtl_start_lock = false;  
6.     set_current_position_item(&_mission_item);  
7.     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();  
8.     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);  
9.   
10.     /* check if we are pretty close to home already */  
11.     float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,  
12.             _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);  
13.   
14.     /* decide where to enter the RTL procedure when we switch into it */  
15.     if (_rtl_state == RTL_STATE_NONE) {  
16.         /* for safety reasons don't go into RTL if landed */  
17.         if (_navigator->get_land_detected()->landed) {  
18.             _rtl_state = RTL_STATE_LANDED;  
19.             mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");  
20.   
21.         /* if lower than return altitude, climb up first */  
22.         } else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt  
23.                + _param_return_alt.get()) {  
24.             _rtl_state = RTL_STATE_CLIMB;  
25.   
26.         /* otherwise go straight to return */  
27.         } else {  
28.             /* set altitude setpoint to current altitude */  
29.             _rtl_state = RTL_STATE_RETURN;  
30.             _mission_item.altitude_is_relative = false;  
31.             _mission_item.altitude = _navigator->get_global_position()->alt;  
32.         }  
33.   
34.     }  
35.   
36.     set_rtl_item();  
37. }  

voidRTL::on_activation(){/* reset starting point so we override what the triplet contained from the previous navigation state */_rtl_start_lock = false;set_current_position_item(&_mission_item);struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);/* check if we are pretty close to home already */float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);/* decide where to enter the RTL procedure when we switch into it */if (_rtl_state == RTL_STATE_NONE) {/* for safety reasons don't go into RTL if landed */if (_navigator->get_land_detected()->landed) {_rtl_state = RTL_STATE_LANDED;mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");/* if lower than return altitude, climb up first */} else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt   + _param_return_alt.get()) {_rtl_state = RTL_STATE_CLIMB;/* otherwise go straight to return */} else {/* set altitude setpoint to current altitude */_rtl_state = RTL_STATE_RETURN;_mission_item.altitude_is_relative = false;_mission_item.altitude = _navigator->get_global_position()->alt;}}set_rtl_item();}

[cpp] view plain copy print?

1. void  
2. RTL::on_active()  
3. {  
4.     if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {  
5.         advance_rtl();  
6.         set_rtl_item();  
7.     }  
8. }  

voidRTL::on_active(){if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {advance_rtl();set_rtl_item();}}

这两个函数都好理解，前面都是为了产生_rtl_state，以便set_rtl_item();调用

[cpp] view plain copy print?

1. void  
2. RTL::set_rtl_item()  
3. {  
4.     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();  
5.     /* make sure we have the latest params */  
6.     updateParams();  
7.     if (!_rtl_start_lock) {  
8.         set_previous_pos_setpoint();  
9.     }  
10.     _navigator->set_can_loiter_at_sp(false);  
11.     switch (_rtl_state) {  
12.     case RTL_STATE_CLIMB: {  
13.         float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();  
14.         _mission_item.lat = _navigator->get_global_position()->lat;  
15.         _mission_item.lon = _navigator->get_global_position()->lon;  
16.         _mission_item.altitude_is_relative = false;  
17.         _mission_item.altitude = climb_alt;  
18.         _mission_item.yaw = NAN;  
19.         _mission_item.loiter_radius = _navigator->get_loiter_radius();  
20.         _mission_item.loiter_direction = 1;  
21.         _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
22.         _mission_item.acceptance_radius = _navigator->get_acceptance_radius();  
23.         _mission_item.time_inside = 0.0f;  
24.         _mission_item.pitch_min = 0.0f;  
25.         _mission_item.autocontinue = true;  
26.         _mission_item.origin = ORIGIN_ONBOARD;  
27.         mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",  
28.             (int)(climb_alt),  
29.             (int)(climb_alt - _navigator->get_home_position()->alt));  
30.         break;  
31.     }  
32.     case RTL_STATE_RETURN: {  
33.         _mission_item.lat = _navigator->get_home_position()->lat;  
34.         _mission_item.lon = _navigator->get_home_position()->lon;  
35.         // don't change altitude  
36.   
37.         // use home yaw if close to home  
38.         /* check if we are pretty close to home already */  
39.         float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,  
40.                 _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);  
41.         if (home_dist < _param_rtl_min_dist.get()) {  
42.             _mission_item.yaw = _navigator->get_home_position()->yaw;  
43.         } else {  
44.             if (pos_sp_triplet->previous.valid) {  
45.                 /* if previous setpoint is valid then use it to calculate heading to home */  
46.                 _mission_item.yaw = get_bearing_to_next_waypoint(  
47.                         pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,  
48.                         _mission_item.lat, _mission_item.lon);  
49.             } else {  
50.                 /* else use current position */  
51.                 _mission_item.yaw = get_bearing_to_next_waypoint(  
52.                         _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,  
53.                         _mission_item.lat, _mission_item.lon);  
54.             }  
55.         }  
56.         _mission_item.loiter_radius = _navigator->get_loiter_radius();  
57.         _mission_item.loiter_direction = 1;  
58.         _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
59.         _mission_item.acceptance_radius = _navigator->get_acceptance_radius();  
60.         _mission_item.time_inside = 0.0f;  
61.         _mission_item.pitch_min = 0.0f;  
62.         _mission_item.autocontinue = true;  
63.         _mission_item.origin = ORIGIN_ONBOARD;  
64.         mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",  
65.             (int)(_mission_item.altitude),  
66.             (int)(_mission_item.altitude - _navigator->get_home_position()->alt));  
67.         _rtl_start_lock = true;  
68.         break;  
69.     }  
70.     case RTL_STATE_TRANSITION_TO_MC: {  
71.         _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;  
72.         _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;  
73.         break;  
74.     }  
75.     case RTL_STATE_DESCEND: {  
76.         _mission_item.lat = _navigator->get_home_position()->lat;  
77.         _mission_item.lon = _navigator->get_home_position()->lon;  
78.         _mission_item.altitude_is_relative = false;  
79.         _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();  
80.         // check if we are already lower - then we will just stay there  
81.         if (_mission_item.altitude > _navigator->get_global_position()->alt) {  
82.             _mission_item.altitude = _navigator->get_global_position()->alt;  
83.         }  
84.         _mission_item.yaw = _navigator->get_home_position()->yaw;  
85.         // except for vtol which might be still off here and should point towards this location  
86.         float d_current = get_distance_to_next_waypoint(  
87.             _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,  
88.             _mission_item.lat, _mission_item.lon);  
89.   
90.         if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {  
91.             _mission_item.yaw = get_bearing_to_next_waypoint(  
92.                 _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,  
93.                 _mission_item.lat, _mission_item.lon);  
94.         }  
95.         _mission_item.loiter_radius = _navigator->get_loiter_radius();  
96.         _mission_item.loiter_direction = 1;  
97.         _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
98.         _mission_item.acceptance_radius = _navigator->get_acceptance_radius();  
99.         _mission_item.time_inside = 0.0f;  
100.         _mission_item.pitch_min = 0.0f;  
101.         _mission_item.autocontinue = false;  
102.         _mission_item.origin = ORIGIN_ONBOARD;  
103.         /* disable previous setpoint to prevent drift */  
104.         pos_sp_triplet->previous.valid = false;  
105.         mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",  
106.             (int)(_mission_item.altitude),  
107.             (int)(_mission_item.altitude - _navigator->get_home_position()->alt));  
108.         break;  
109.     }  
110.     case RTL_STATE_LOITER: {  
111.         bool autoland = _param_land_delay.get() > -DELAY_SIGMA;  
112.   
113.         _mission_item.lat = _navigator->get_home_position()->lat;  
114.         _mission_item.lon = _navigator->get_home_position()->lon;  
115.         // don't change altitude  
116.         _mission_item.yaw = _navigator->get_home_position()->yaw;  
117.         _mission_item.loiter_radius = _navigator->get_loiter_radius();  
118.         _mission_item.loiter_direction = 1;  
119.         _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;  
120.         _mission_item.acceptance_radius = _navigator->get_acceptance_radius();  
121.         _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();  
122.         _mission_item.pitch_min = 0.0f;  
123.         _mission_item.autocontinue = autoland;  
124.         _mission_item.origin = ORIGIN_ONBOARD;  
125.         _navigator->set_can_loiter_at_sp(true);  
126.         if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {  
127.             mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);  
128.         } else {  
129.             mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");  
130.         }  
131.         break;  
132.     }  
133.     case RTL_STATE_LAND: {  
134.         _mission_item.yaw = _navigator->get_home_position()->yaw;  
135.         set_land_item(&_mission_item, false);  
136.   
137.         mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");  
138.         break;  
139.     }  
140.     case RTL_STATE_LANDED: {  
141.         set_idle_item(&_mission_item);  
142.   
143.         mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");  
144.         break;  
145.     }  
146.     default:  
147.         break;  
148.     }  
149.     reset_mission_item_reached();  
150.     /* execute command if set */  
151.     if (!item_contains_position(&_mission_item)) {  
152.         issue_command(&_mission_item);  
153.     }  
154.     /* convert mission item to current position setpoint and make it valid */  
155.     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);  
156.     pos_sp_triplet->next.valid = false;  
157.     _navigator->set_position_setpoint_triplet_updated();  
158. }  

voidRTL::set_rtl_item(){struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();/* make sure we have the latest params */updateParams();if (!_rtl_start_lock) {set_previous_pos_setpoint();}_navigator->set_can_loiter_at_sp(false);switch (_rtl_state) {case RTL_STATE_CLIMB: {float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();_mission_item.lat = _navigator->get_global_position()->lat;_mission_item.lon = _navigator->get_global_position()->lon;_mission_item.altitude_is_relative = false;_mission_item.altitude = climb_alt;_mission_item.yaw = NAN;_mission_item.loiter_radius = _navigator->get_loiter_radius();_mission_item.loiter_direction = 1;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.acceptance_radius = _navigator->get_acceptance_radius();_mission_item.time_inside = 0.0f;_mission_item.pitch_min = 0.0f;_mission_item.autocontinue = true;_mission_item.origin = ORIGIN_ONBOARD;mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",(int)(climb_alt),(int)(climb_alt - _navigator->get_home_position()->alt));break;}case RTL_STATE_RETURN: {_mission_item.lat = _navigator->get_home_position()->lat;_mission_item.lon = _navigator->get_home_position()->lon;// don't change altitude// use home yaw if close to home/* check if we are pretty close to home already */float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);if (home_dist < _param_rtl_min_dist.get()) {_mission_item.yaw = _navigator->get_home_position()->yaw;} else {if (pos_sp_triplet->previous.valid) {/* if previous setpoint is valid then use it to calculate heading to home */_mission_item.yaw = get_bearing_to_next_waypoint(        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,        _mission_item.lat, _mission_item.lon);} else {/* else use current position */_mission_item.yaw = get_bearing_to_next_waypoint(        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,        _mission_item.lat, _mission_item.lon);}}_mission_item.loiter_radius = _navigator->get_loiter_radius();_mission_item.loiter_direction = 1;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.acceptance_radius = _navigator->get_acceptance_radius();_mission_item.time_inside = 0.0f;_mission_item.pitch_min = 0.0f;_mission_item.autocontinue = true;_mission_item.origin = ORIGIN_ONBOARD;mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",(int)(_mission_item.altitude),(int)(_mission_item.altitude - _navigator->get_home_position()->alt));_rtl_start_lock = true;break;}case RTL_STATE_TRANSITION_TO_MC: {_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;break;}case RTL_STATE_DESCEND: {_mission_item.lat = _navigator->get_home_position()->lat;_mission_item.lon = _navigator->get_home_position()->lon;_mission_item.altitude_is_relative = false;_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();// check if we are already lower - then we will just stay thereif (_mission_item.altitude > _navigator->get_global_position()->alt) {_mission_item.altitude = _navigator->get_global_position()->alt;}_mission_item.yaw = _navigator->get_home_position()->yaw;// except for vtol which might be still off here and should point towards this locationfloat d_current = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,_mission_item.lat, _mission_item.lon);if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,_mission_item.lat, _mission_item.lon);}_mission_item.loiter_radius = _navigator->get_loiter_radius();_mission_item.loiter_direction = 1;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.acceptance_radius = _navigator->get_acceptance_radius();_mission_item.time_inside = 0.0f;_mission_item.pitch_min = 0.0f;_mission_item.autocontinue = false;_mission_item.origin = ORIGIN_ONBOARD;/* disable previous setpoint to prevent drift */pos_sp_triplet->previous.valid = false;mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",(int)(_mission_item.altitude),(int)(_mission_item.altitude - _navigator->get_home_position()->alt));break;}case RTL_STATE_LOITER: {bool autoland = _param_land_delay.get() > -DELAY_SIGMA;_mission_item.lat = _navigator->get_home_position()->lat;_mission_item.lon = _navigator->get_home_position()->lon;// don't change altitude_mission_item.yaw = _navigator->get_home_position()->yaw;_mission_item.loiter_radius = _navigator->get_loiter_radius();_mission_item.loiter_direction = 1;_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;_mission_item.acceptance_radius = _navigator->get_acceptance_radius();_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();_mission_item.pitch_min = 0.0f;_mission_item.autocontinue = autoland;_mission_item.origin = ORIGIN_ONBOARD;_navigator->set_can_loiter_at_sp(true);if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);} else {mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");}break;}case RTL_STATE_LAND: {_mission_item.yaw = _navigator->get_home_position()->yaw;set_land_item(&_mission_item, false);mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");break;}case RTL_STATE_LANDED: {set_idle_item(&_mission_item);mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");break;}default:break;}reset_mission_item_reached();/* execute command if set */if (!item_contains_position(&_mission_item)) {issue_command(&_mission_item);}/* convert mission item to current position setpoint and make it valid */mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);pos_sp_triplet->next.valid = false;_navigator->set_position_setpoint_triplet_updated();}

在set_rtl_item();中，前面是给_mission_item结构体赋值，mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);是将_mission_item结构体的值赋给pos_sp_triplet结构体

[cpp] view plain copy print?

1. void  
2. MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)  
3. {  
4.     /* set the correct setpoint for vtol transition */  
5.   
6.     if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)  
7.             && item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {  
8.         sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;  
9.         waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,  
10.                                            _navigator->get_global_position()->lon,  
11.                                            item->yaw,  
12.                                            1000000.0f,  
13.                                            &sp->lat,  
14.                                            &sp->lon);  
15.         sp->alt = _navigator->get_global_position()->alt;  
16.     }  
17.   
18.   
19.     /* don't change the setpoint for non-position items */  
20.     if (!item_contains_position(item)) {  
21.         return;  
22.     }  
23.   
24.     sp->valid = true;  
25.     sp->lat = item->lat;  
26.     sp->lon = item->lon;  
27.     sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;  
28.     sp->yaw = item->yaw;  
29.     sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :  
30.                 _navigator->get_loiter_radius();  
31.     sp->loiter_direction = item->loiter_direction;  
32.     sp->pitch_min = item->pitch_min;  
33.     sp->acceptance_radius = item->acceptance_radius;  
34.     sp->disable_mc_yaw_control = false;  
35.     sp->cruising_speed = _navigator->get_cruising_speed();  
36.   
37.     switch (item->nav_cmd) {  
38.     case NAV_CMD_IDLE:  
39.         sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;  
40.         break;  
41.   
42.     case NAV_CMD_TAKEOFF:  
43.     case NAV_CMD_VTOL_TAKEOFF:  
44.         sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;  
45.         break;  
46.   
47.     case NAV_CMD_LAND:  
48.     case NAV_CMD_VTOL_LAND:  
49.         sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;  
50.         if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){  
51.             sp->disable_mc_yaw_control = true;  
52.         }  
53.         break;  
54.   
55.     case NAV_CMD_LOITER_TIME_LIMIT:  
56.     case NAV_CMD_LOITER_TURN_COUNT:  
57.     case NAV_CMD_LOITER_UNLIMITED:  
58.         sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;  
59.         if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){  
60.             sp->disable_mc_yaw_control = true;  
61.         }  
62.         break;  
63.   
64.     default:  
65.         sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;  
66.         break;  
67.     }  
68. }  

voidMissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp){/* set the correct setpoint for vtol transition */if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,   _navigator->get_global_position()->lon,   item->yaw,   1000000.0f,   &sp->lat,   &sp->lon);sp->alt = _navigator->get_global_position()->alt;}/* don't change the setpoint for non-position items */if (!item_contains_position(item)) {return;}sp->valid = true;sp->lat = item->lat;sp->lon = item->lon;sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;sp->yaw = item->yaw;sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :_navigator->get_loiter_radius();sp->loiter_direction = item->loiter_direction;sp->pitch_min = item->pitch_min;sp->acceptance_radius = item->acceptance_radius;sp->disable_mc_yaw_control = false;sp->cruising_speed = _navigator->get_cruising_speed();switch (item->nav_cmd) {case NAV_CMD_IDLE:sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;break;case NAV_CMD_TAKEOFF:case NAV_CMD_VTOL_TAKEOFF:sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;break;case NAV_CMD_LAND:case NAV_CMD_VTOL_LAND:sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){sp->disable_mc_yaw_control = true;}break;case NAV_CMD_LOITER_TIME_LIMIT:case NAV_CMD_LOITER_TURN_COUNT:case NAV_CMD_LOITER_UNLIMITED:sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){sp->disable_mc_yaw_control = true;}break;default:sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;break;}}

[cpp] view plain copy print?

1. if (_pos_sp_triplet_updated) {  
2.     publish_position_setpoint_triplet();  
3.     _pos_sp_triplet_updated = false;  
4. }  

if (_pos_sp_triplet_updated) {publish_position_setpoint_triplet();_pos_sp_triplet_updated = false;}

[cpp] view plain copy print?

1. void  
2. Navigator::publish_position_setpoint_triplet()  
3. {  
4.     /* update navigation state */  
5.     _pos_sp_triplet.nav_state = _vstatus.nav_state;  
6.   
7.     /* do not publish an empty triplet */  
8.     if (!_pos_sp_triplet.current.valid) {  
9.         return;  
10.     }  
11.   
12.     /* lazily publish the position setpoint triplet only once available */  
13.     if (_pos_sp_triplet_pub != nullptr) {  
14.         orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);  
15.   
16.     } else {  
17.         _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);  
18.     }  
19. }  

voidNavigator::publish_position_setpoint_triplet(){/* update navigation state */_pos_sp_triplet.nav_state = _vstatus.nav_state;/* do not publish an empty triplet */if (!_pos_sp_triplet.current.valid) {return;}/* lazily publish the position setpoint triplet only once available */if (_pos_sp_triplet_pub != nullptr) {orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);} else {_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);}}

发布ORB_ID(position_setpoint_triplet)进而用于位置控制mc_pos_control_main.cpp里面的control_auto()函数

[cpp] view plain copy print?

1. void MulticopterPositionControl::control_auto(float dt)  
2. {  
3.     ……  
4.     if (updated) {  
5.         orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);  
6.         //Make sure that the position setpoint is valid  
7.         if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||  
8.                 !PX4_ISFINITE(_pos_sp_triplet.current.lon) ||  
9.                 !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {  
10.             _pos_sp_triplet.current.valid = false;  
11.         }  
12.     }  
13.     ……  
14. }  

void MulticopterPositionControl::control_auto(float dt){……if (updated) {orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);//Make sure that the position setpoint is validif (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {_pos_sp_triplet.current.valid = false;}}……}

接着分析mission.cpp

run()函数中第一次运行on_activation();，以后就运行on_active();

[cpp] view plain copy print?

1. void  
2. NavigatorMode::run(bool active)  
3. {  
4.     if (active) {  
5.         if (_first_run) {  
6.             /* first run */  
7.             _first_run = false;  
8.             /* Reset stay in failsafe flag */  
9.             _navigator->get_mission_result()->stay_in_failsafe = false;  
10.             _navigator->set_mission_result_updated();  
11.             on_activation();  
12.   
13.         } else {  
14.             /* periodic updates when active */  
15.             on_active();  
16.         }  
17.   
18.     } else {  
19.         /* periodic updates when inactive */  
20.         _first_run = true;  
21.         on_inactive();  
22.     }  
23. }  

voidNavigatorMode::run(bool active){if (active) {if (_first_run) {/* first run */_first_run = false;/* Reset stay in failsafe flag */_navigator->get_mission_result()->stay_in_failsafe = false;_navigator->set_mission_result_updated();on_activation();} else {/* periodic updates when active */on_active();}} else {/* periodic updates when inactive */_first_run = true;on_inactive();}}

[cpp] view plain copy print?

1. void  
2. Mission::on_activation()  
3. {  
4.     set_mission_items();  
5. }  

voidMission::on_activation(){set_mission_items();}

[cpp] view plain copy print?

1. void  
2. Mission::on_active()  
3. {  
4.     check_mission_valid();  
5.   
6.     /* check if anything has changed */  
7.     bool onboard_updated = false;  
8.     orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);  
9.     if (onboard_updated) {  
10.         update_onboard_mission();  
11.     }  
12.     bool offboard_updated = false;  
13.     orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);  
14.     if (offboard_updated) {  
15.         update_offboard_mission();  
16.     }  
17.     /* reset the current offboard mission if needed */  
18.     if (need_to_reset_mission(true)) {  
19.         reset_offboard_mission(_offboard_mission);  
20.         update_offboard_mission();  
21.         offboard_updated = true;  
22.     }  
23.     /* reset mission items if needed */  
24.     if (onboard_updated || offboard_updated) {  
25.         set_mission_items();  
26.     }  
27.     /* lets check if we reached the current mission item */  
28.     if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {  
29.         set_mission_item_reached();  
30.         if (_mission_item.autocontinue) {  
31.             /* switch to next waypoint if 'autocontinue' flag set */  
32.             advance_mission();  
33.             set_mission_items();  
34.         }  
35.     } else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {  
36.         altitude_sp_foh_update();  
37.     } else {  
38.         /* if waypoint position reached allow loiter on the setpoint */  
39.         if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {  
40.             _navigator->set_can_loiter_at_sp(true);  
41.         }  
42.     }  
43.     /* see if we need to update the current yaw heading */  
44.     if ((_param_yawmode.get() != MISSION_YAWMODE_NONE  
45.             && _param_yawmode.get() < MISSION_YAWMODE_MAX  
46.             && _mission_type != MISSION_TYPE_NONE)  
47.             || _navigator->get_vstatus()->is_vtol) {  
48.         heading_sp_update();  
49.     }  
50. }  

voidMission::on_active(){check_mission_valid();/* check if anything has changed */bool onboard_updated = false;orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);if (onboard_updated) {update_onboard_mission();}bool offboard_updated = false;orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);if (offboard_updated) {update_offboard_mission();}/* reset the current offboard mission if needed */if (need_to_reset_mission(true)) {reset_offboard_mission(_offboard_mission);update_offboard_mission();offboard_updated = true;}/* reset mission items if needed */if (onboard_updated || offboard_updated) {set_mission_items();}/* lets check if we reached the current mission item */if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {set_mission_item_reached();if (_mission_item.autocontinue) {/* switch to next waypoint if 'autocontinue' flag set */advance_mission();set_mission_items();}} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {altitude_sp_foh_update();} else {/* if waypoint position reached allow loiter on the setpoint */if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {_navigator->set_can_loiter_at_sp(true);}}/* see if we need to update the current yaw heading */if ((_param_yawmode.get() != MISSION_YAWMODE_NONE&& _param_yawmode.get() < MISSION_YAWMODE_MAX&& _mission_type != MISSION_TYPE_NONE)|| _navigator->get_vstatus()->is_vtol) {heading_sp_update();}}

这里都为调用set_mission_items();做准备

[cpp] view plain copy print?

1. void  
2. Mission::set_mission_items()  
3. {  
4.     /* make sure param is up to date */  
5.     updateParams();  
6.   
7.     /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */  
8.     altitude_sp_foh_reset();  
9.   
10.     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();  
11.   
12.     /* the home dist check provides user feedback, so we initialize it to this */  
13.     bool user_feedback_done = false;  
14.   
15.     /* mission item that comes after current if available */  
16.     struct mission_item_s mission_item_next_position;  
17.     bool has_next_position_item = false;  
18.   
19.     work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
20.   
21.     /* copy information about the previous mission item */  
22.     if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {  
23.         /* Copy previous mission item altitude */  
24.         _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);  
25.     }  
26.   
27.     /* try setting onboard mission item */  
28.     if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {  
29.         /* if mission type changed, notify */  
30.         if (_mission_type != MISSION_TYPE_ONBOARD) {  
31.             mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");  
32.             user_feedback_done = true;  
33.         }  
34.         _mission_type = MISSION_TYPE_ONBOARD;  
35.   
36.     /* try setting offboard mission item */  
37.     } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {  
38.         /* if mission type changed, notify */  
39.         if (_mission_type != MISSION_TYPE_OFFBOARD) {  
40.             mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");  
41.             user_feedback_done = true;  
42.         }  
43.         _mission_type = MISSION_TYPE_OFFBOARD;  
44.     } else {  
45.         /* no mission available or mission finished, switch to loiter */  
46.         if (_mission_type != MISSION_TYPE_NONE) {  
47.             /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */  
48.             mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");  
49.             user_feedback_done = true;  
50.   
51.             /* use last setpoint for loiter */  
52.             _navigator->set_can_loiter_at_sp(true);  
53.   
54.         }  
55.   
56.         _mission_type = MISSION_TYPE_NONE;  
57.   
58.         /* set loiter mission item and ensure that there is a minimum clearance from home */  
59.         set_loiter_item(&_mission_item, _param_takeoff_alt.get());  
60.   
61.         /* update position setpoint triplet  */  
62.         pos_sp_triplet->previous.valid = false;  
63.         mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);  
64.         pos_sp_triplet->next.valid = false;  
65.   
66.         /* reuse setpoint for LOITER only if it's not IDLE */  
67.         _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);  
68.   
69.         set_mission_finished();  
70.   
71.         if (!user_feedback_done) {  
72.             /* only tell users that we got no mission if there has not been any 
73.              * better, more specific feedback yet 
74.              * https://en.wikipedia.org/wiki/Loiter_(aeronautics) 
75.              */  
76.   
77.             if (_navigator->get_land_detected()->landed) {  
78.                 /* landed, refusing to take off without a mission */  
79.   
80.                 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");  
81.             } else {  
82.                 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");  
83.             }  
84.   
85.             user_feedback_done = true;  
86.   
87.         }  
88.   
89.         _navigator->set_position_setpoint_triplet_updated();  
90.         return;  
91.     }  
92.   
93.     /*********************************** handle mission item *********************************************/  
94.   
95.     /* handle position mission items */  
96.   
97.   
98.     if (item_contains_position(&_mission_item)) {  
99.   
100.         /* force vtol land */  
101.         if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get()  
102.                 && !_navigator->get_vstatus()->is_rotary_wing){  
103.             _mission_item.nav_cmd = NAV_CMD_VTOL_LAND;  
104.         }  
105.   
106.         /* we have a new position item so set previous position setpoint to current */  
107.         set_previous_pos_setpoint();  
108.   
109.         /* do takeoff before going to setpoint if needed and not already in takeoff */  
110.         if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {  
111.             new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;  
112.   
113.             /* use current mission item as next position item */  
114.             memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));  
115.             mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;  
116.             has_next_position_item = true;  
117.   
118.             float takeoff_alt = calculate_takeoff_altitude(&_mission_item);  
119.   
120.             mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));  
121.   
122.             _mission_item.nav_cmd = NAV_CMD_TAKEOFF;  
123.             _mission_item.lat = _navigator->get_global_position()->lat;  
124.             _mission_item.lon = _navigator->get_global_position()->lon;  
125.             /* ignore yaw for takeoff items */  
126.             _mission_item.yaw = NAN;  
127.             _mission_item.altitude = takeoff_alt;  
128.             _mission_item.altitude_is_relative = false;  
129.             _mission_item.autocontinue = true;  
130.             _mission_item.time_inside = 0;  
131.         }  
132.   
133.         /* if we just did a takeoff navigate to the actual waypoint now */  
134.         if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {  
135.   
136.             if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF  
137.                     && _navigator->get_vstatus()->is_rotary_wing  
138.                     && !_navigator->get_land_detected()->landed  
139.                     && has_next_position_item) {  
140.                 /* check if the vtol_takeoff command is on top of us */  
141.                 if(do_need_move_to_takeoff()){  
142.                     new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;  
143.                 } else {  
144.                     new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
145.                 }  
146.   
147.   
148.                 _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;  
149.                 _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;  
150.                 _mission_item.yaw = _navigator->get_global_position()->yaw;  
151.             } else {  
152.                 new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
153.                 _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
154.                 /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */  
155.                 _mission_item.yaw = NAN;  
156.             }  
157.   
158.         }  
159.   
160.         /* takeoff completed and transitioned, move to takeoff wp as fixed wing */  
161.         if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF  
162.                 && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {  
163.   
164.             new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
165.             _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
166.             _mission_item.autocontinue = true;  
167.             _mission_item.time_inside = 0.0f;  
168.         }  
169.   
170.         /* move to land wp as fixed wing */  
171.         if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND  
172.                 && _work_item_type == WORK_ITEM_TYPE_DEFAULT  
173.                 && !_navigator->get_land_detected()->landed) {  
174.             new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;  
175.             /* use current mission item as next position item */  
176.             memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));  
177.             has_next_position_item = true;  
178.             float altitude = _navigator->get_global_position()->alt;  
179.             if (pos_sp_triplet->current.valid) {  
180.                 altitude = pos_sp_triplet->current.alt;  
181.             }  
182.   
183.             _mission_item.altitude = altitude;  
184.             _mission_item.altitude_is_relative = false;  
185.             _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
186.             _mission_item.autocontinue = true;  
187.             _mission_item.time_inside = 0;  
188.         }  
189.   
190.         /* transition to MC */  
191.         if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND  
192.                 && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND  
193.                 && !_navigator->get_vstatus()->is_rotary_wing  
194.                 && !_navigator->get_land_detected()->landed) {  
195.             _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;  
196.             _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;  
197.             _mission_item.autocontinue = true;  
198.             new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;  
199.         }  
200.   
201.         /* move to landing waypoint before descent if necessary */  
202.         if (do_need_move_to_land() &&  
203.                 (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||  
204.                  _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {  
205.             new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;  
206.   
207.             /* use current mission item as next position item */  
208.             memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));  
209.             has_next_position_item = true;  
210.   
211.             /* 
212.              * Ignoring waypoint altitude: 
213.              * Set altitude to the same as we have now to prevent descending too fast into 
214.              * the ground. Actual landing will descend anyway until it touches down. 
215.              * XXX: We might want to change that at some point if it is clear to the user 
216.              * what the altitude means on this waypoint type. 
217.              */  
218.             float altitude = _navigator->get_global_position()->alt;  
219.   
220.             _mission_item.altitude = altitude;  
221.             _mission_item.altitude_is_relative = false;  
222.             _mission_item.nav_cmd = NAV_CMD_WAYPOINT;  
223.             _mission_item.autocontinue = true;  
224.             _mission_item.time_inside = 0;  
225.         }  
226.   
227.         /* we just moved to the landing waypoint, now descend */  
228.         if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND  
229.                 && _navigator->get_vstatus()->is_rotary_wing) {  
230.             new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
231.         }  
232.   
233.   
234.   
235.         /* ignore yaw for landing items */  
236.         /* XXX: if specified heading for landing is desired we could add another step before the descent 
237.          * that aligns the vehicle first */  
238.         if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) {  
239.             _mission_item.yaw = NAN;  
240.         }  
241.   
242.     /* handle non-position mission items such as commands */  
243.     } else {  
244.   
245.         /* turn towards next waypoint before MC to FW transition */  
246.         if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION  
247.                 && _work_item_type != WORK_ITEM_TYPE_ALIGN  
248.                 && _navigator->get_vstatus()->is_rotary_wing  
249.                 && !_navigator->get_land_detected()->landed  
250.                 && has_next_position_item) {  
251.   
252.             new_work_item_type = WORK_ITEM_TYPE_ALIGN;  
253.             set_align_mission_item(&_mission_item, &mission_item_next_position);  
254.         }  
255.   
256.         /* yaw is aligned now */  
257.         if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {  
258.             new_work_item_type = WORK_ITEM_TYPE_DEFAULT;  
259.         }  
260.   
261.     }  
262.   
263.     /*********************************** set setpoints and check next *********************************************/  
264.   
265.     /* set current position setpoint from mission item (is protected agains non-position items) */  
266.     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);  
267.   
268.     /* issue command if ready (will do nothing for position mission items) */  
269.     issue_command(&_mission_item);  
270.   
271.     /* set current work item type */  
272.     _work_item_type = new_work_item_type;  
273.   
274.     /* require takeoff after landing or idle */  
275.     if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {  
276.         _need_takeoff = true;  
277.     }  
278.   
279.     _navigator->set_can_loiter_at_sp(false);  
280.     reset_mission_item_reached();  
281.   
282.     if (_mission_type == MISSION_TYPE_OFFBOARD) {  
283.         set_current_offboard_mission_item();  
284.     }  
285.     // TODO: report onboard mission item somehow  
286.   
287.     if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {  
288.         /* try to process next mission item */  
289.   
290.         if (has_next_position_item) {  
291.             /* got next mission item, update setpoint triplet */  
292.             mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);  
293.         } else {  
294.             /* next mission item is not available */  
295.             pos_sp_triplet->next.valid = false;  
296.         }  
297.   
298.     } else {  
299.         /* vehicle will be paused on current waypoint, don't set next item */  
300.         pos_sp_triplet->next.valid = false;  
301.     }  
302.   
303.     /* Save the distance between the current sp and the previous one */  
304.     if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {  
305.         _distance_current_previous = get_distance_to_next_waypoint(  
306.                 pos_sp_triplet->current.lat,  
307.                 pos_sp_triplet->current.lon,  
308.                 pos_sp_triplet->previous.lat,  
309.                 pos_sp_triplet->previous.lon);  
310.     }  
311.   
312.     _navigator->set_position_setpoint_triplet_updated();  
313. }  

voidMission::set_mission_items(){/* make sure param is up to date */updateParams();/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */altitude_sp_foh_reset();struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();/* the home dist check provides user feedback, so we initialize it to this */bool user_feedback_done = false;/* mission item that comes after current if available */struct mission_item_s mission_item_next_position;bool has_next_position_item = false;work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;/* copy information about the previous mission item */if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {/* Copy previous mission item altitude */_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);}/* try setting onboard mission item */if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {/* if mission type changed, notify */if (_mission_type != MISSION_TYPE_ONBOARD) {mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");user_feedback_done = true;}_mission_type = MISSION_TYPE_ONBOARD;/* try setting offboard mission item */} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {/* if mission type changed, notify */if (_mission_type != MISSION_TYPE_OFFBOARD) {mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");user_feedback_done = true;}_mission_type = MISSION_TYPE_OFFBOARD;} else {/* no mission available or mission finished, switch to loiter */if (_mission_type != MISSION_TYPE_NONE) {/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");user_feedback_done = true;/* use last setpoint for loiter */_navigator->set_can_loiter_at_sp(true);}_mission_type = MISSION_TYPE_NONE;/* set loiter mission item and ensure that there is a minimum clearance from home */set_loiter_item(&_mission_item, _param_takeoff_alt.get());/* update position setpoint triplet  */pos_sp_triplet->previous.valid = false;mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);pos_sp_triplet->next.valid = false;/* reuse setpoint for LOITER only if it's not IDLE */_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);set_mission_finished();if (!user_feedback_done) {/* only tell users that we got no mission if there has not been any * better, more specific feedback yet * https://en.wikipedia.org/wiki/Loiter_(aeronautics) */if (_navigator->get_land_detected()->landed) {/* landed, refusing to take off without a mission */mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");} else {mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");}user_feedback_done = true;}_navigator->set_position_setpoint_triplet_updated();return;}/*********************************** handle mission item *********************************************//* handle position mission items */if (item_contains_position(&_mission_item)) {/* force vtol land */if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get()&& !_navigator->get_vstatus()->is_rotary_wing){_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;}/* we have a new position item so set previous position setpoint to current */set_previous_pos_setpoint();/* do takeoff before going to setpoint if needed and not already in takeoff */if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;/* use current mission item as next position item */memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;has_next_position_item = true;float takeoff_alt = calculate_takeoff_altitude(&_mission_item);mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));_mission_item.nav_cmd = NAV_CMD_TAKEOFF;_mission_item.lat = _navigator->get_global_position()->lat;_mission_item.lon = _navigator->get_global_position()->lon;/* ignore yaw for takeoff items */_mission_item.yaw = NAN;_mission_item.altitude = takeoff_alt;_mission_item.altitude_is_relative = false;_mission_item.autocontinue = true;_mission_item.time_inside = 0;}/* if we just did a takeoff navigate to the actual waypoint now */if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF&& _navigator->get_vstatus()->is_rotary_wing&& !_navigator->get_land_detected()->landed&& has_next_position_item) {/* check if the vtol_takeoff command is on top of us */if(do_need_move_to_takeoff()){new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;} else {new_work_item_type = WORK_ITEM_TYPE_DEFAULT;}_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;_mission_item.yaw = _navigator->get_global_position()->yaw;} else {new_work_item_type = WORK_ITEM_TYPE_DEFAULT;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */_mission_item.yaw = NAN;}}/* takeoff completed and transitioned, move to takeoff wp as fixed wing */if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {new_work_item_type = WORK_ITEM_TYPE_DEFAULT;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.autocontinue = true;_mission_item.time_inside = 0.0f;}/* move to land wp as fixed wing */if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND&& _work_item_type == WORK_ITEM_TYPE_DEFAULT&& !_navigator->get_land_detected()->landed) {new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;/* use current mission item as next position item */memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));has_next_position_item = true;float altitude = _navigator->get_global_position()->alt;if (pos_sp_triplet->current.valid) {altitude = pos_sp_triplet->current.alt;}_mission_item.altitude = altitude;_mission_item.altitude_is_relative = false;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.autocontinue = true;_mission_item.time_inside = 0;}/* transition to MC */if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND&& !_navigator->get_vstatus()->is_rotary_wing&& !_navigator->get_land_detected()->landed) {_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;_mission_item.autocontinue = true;new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;}/* move to landing waypoint before descent if necessary */if (do_need_move_to_land() &&(_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;/* use current mission item as next position item */memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));has_next_position_item = true;/* * Ignoring waypoint altitude: * Set altitude to the same as we have now to prevent descending too fast into * the ground. Actual landing will descend anyway until it touches down. * XXX: We might want to change that at some point if it is clear to the user * what the altitude means on this waypoint type. */float altitude = _navigator->get_global_position()->alt;_mission_item.altitude = altitude;_mission_item.altitude_is_relative = false;_mission_item.nav_cmd = NAV_CMD_WAYPOINT;_mission_item.autocontinue = true;_mission_item.time_inside = 0;}/* we just moved to the landing waypoint, now descend */if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND&& _navigator->get_vstatus()->is_rotary_wing) {new_work_item_type = WORK_ITEM_TYPE_DEFAULT;}/* ignore yaw for landing items *//* XXX: if specified heading for landing is desired we could add another step before the descent * that aligns the vehicle first */if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) {_mission_item.yaw = NAN;}/* handle non-position mission items such as commands */} else {/* turn towards next waypoint before MC to FW transition */if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION&& _work_item_type != WORK_ITEM_TYPE_ALIGN&& _navigator->get_vstatus()->is_rotary_wing&& !_navigator->get_land_detected()->landed&& has_next_position_item) {new_work_item_type = WORK_ITEM_TYPE_ALIGN;set_align_mission_item(&_mission_item, &mission_item_next_position);}/* yaw is aligned now */if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {new_work_item_type = WORK_ITEM_TYPE_DEFAULT;}}/*********************************** set setpoints and check next *********************************************//* set current position setpoint from mission item (is protected agains non-position items) */mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);/* issue command if ready (will do nothing for position mission items) */issue_command(&_mission_item);/* set current work item type */_work_item_type = new_work_item_type;/* require takeoff after landing or idle */if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {_need_takeoff = true;}_navigator->set_can_loiter_at_sp(false);reset_mission_item_reached();if (_mission_type == MISSION_TYPE_OFFBOARD) {set_current_offboard_mission_item();}// TODO: report onboard mission item somehowif (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {/* try to process next mission item */if (has_next_position_item) {/* got next mission item, update setpoint triplet */mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);} else {/* next mission item is not available */pos_sp_triplet->next.valid = false;}} else {/* vehicle will be paused on current waypoint, don't set next item */pos_sp_triplet->next.valid = false;}/* Save the distance between the current sp and the previous one */if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,pos_sp_triplet->current.lon,pos_sp_triplet->previous.lat,pos_sp_triplet->previous.lon);}_navigator->set_position_setpoint_triplet_updated();}

好吧，这个函数有点复杂，但最终目的还是为了使用

mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);

将_mission_item结构体的值赋给pos_sp_triplet结构体
之后在navigator_main.cpp中调用

[cpp] view plain copy print?

1. if (_pos_sp_triplet_updated) {  
2.     publish_position_setpoint_triplet();  
3.     _pos_sp_triplet_updated = false;  
4. }  

if (_pos_sp_triplet_updated) {publish_position_setpoint_triplet();_pos_sp_triplet_updated = false;}

最后发布出去给各modules