使用uart得到的位置信息进行起飞
在得到了位置信息的前提下,我们开始进行模拟起飞,即使用usb供电,人工控制其高度,在上位机查看油门大小,电机的pwm输出。
- commander.cpp
在commander.cpp中,主要是判断能不能切换到takeoff模式,并且设置一些标志位,为takeoff做准备 - navigator_main.cpp
在navigator中,只要是通过run()
重载,来确定使用的函数,takeoff模式下当然是进入takeoff.cpp。- 在navigator_main.cpp中有设置takeoff triplet
这里面主要是设置了rep->current ,为takeoff.cpp准备1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35position_setpoint_triplet_s *rep = get_takeoff_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if (home_position_valid()) {
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
} else {
rep->current.yaw = get_local_position()->yaw;
rep->previous.valid = false;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
rep->current.lon = NAN;
}
rep->current.alt = cmd.param7;
rep->current.valid = true;
rep->next.valid = false; - takeoff.cpp
在on_activation()
设置高度数据abs_altitude
,并且需要设置坐标经纬度的数据,由于不使用gps,所以没有经纬度的数据,这里我们将lpe输出的xy数据赋值给经纬度的变量set_takeoff_item(&_mission_item, abs_altitude)
将经纬度的变量赋值给位置期望1
2item->lat = _navigator->get_local_position()->x;
item->lon = _navigator->get_local_position()->y;mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35sp->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->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
_navigator->get_loiter_radius();
sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
sp->acceptance_radius = item.acceptance_radius;
sp->cruising_speed = _navigator->get_cruising_speed();
sp->cruising_throttle = _navigator->get_cruising_throttle();
switch (item.nav_cmd) {
case NAV_CMD_TAKEOFF:
// if already flying (armed and !landed) treat TAKEOFF like regular POSITION
if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !_navigator->get_land_detected()->landed) {
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
} else {
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
// set pitch and ensure that the hold time is zero
sp->pitch_min = item.pitch_min;
}
break;
}
sp->valid = true;
return sp->valid;
- 在navigator_main.cpp中有设置takeoff triplet
position_control.cpp
因为takeoff原本使用的是gps的数据,在位置控制的时候,px4将其进行坐标转化,转化为机体坐标系的数据,在navigator中我们使用的就是机体坐标系的xy数据,所以不需要进行坐标系的转化。
坐标系转化:1
2
3
4
5
6
7
8
9
10//only project setpoints if they are finite, else use current position
if (PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
PX4_ISFINITE(_pos_sp_triplet.current.lon)) {
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_pos_sp.data[0], &curr_pos_sp.data[1]);
_triplet_lat_lon_finite = true;
}改动:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17//only project setpoints if they are finite, else use current position
if (PX4_ISFINITE(_pos_sp_triplet.current.x) &&
PX4_ISFINITE(_pos_sp_triplet.current.y)) {
/* project setpoint to local frame */
curr_pos_sp(0) = _pos_sp_triplet.current.x;
curr_pos_sp(1) = _pos_sp_triplet.current.y;
_triplet_lat_lon_finite = true;
}else if (PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
PX4_ISFINITE(_pos_sp_triplet.current.lon)) {
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_pos_sp.data[0], &curr_pos_sp.data[1]);
_triplet_lat_lon_finite = true;
}模拟飞行
解锁之后应该能看到油门数据突变,变为油门悬停值,当你升高飞行器至起飞高度后,模式切换为loier (待添加图片)起飞之后
起飞之后在on_active()函数中会判断是不是到达了起飞点
is_mission_item_reached()
它的思路是先判断有没有到达起飞高度或者任务点,在判断yaw有没有到,最后判断是不是在航线里。
因为使用的是takeoff模式,所以先判断有没有到达起飞高度,也只需要修改起飞高度,yaw会以当前的yaw为期望,起飞之后yaw一定是满足err的,航线与takeoff模式无关。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {
/* We want to avoid the edge case where the acceptance radius is bigger or equal than
* the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
* take-off procedures like leaving the landing gear down. */
float takeoff_alt = _mission_item.altitude_is_relative ?
_mission_item.altitude :
(_mission_item.altitude - _navigator->get_home_position()->alt);
float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius();
/* It should be safe to just use half of the takoeff_alt as an acceptance radius. */
if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) {
altitude_acceptance_radius = takeoff_alt / 2.0f;
}
/* require only altitude for takeoff for multicopter */
if(_navigator->home_position_valid()){
if (_navigator->get_global_position()->alt >
altitude_amsl - altitude_acceptance_radius) {
_waypoint_position_reached = true;
}
}
else {
if(- _navigator->get_local_position()->z > altitude_amsl -0.2f){
_waypoint_position_reached = true ;
}
}可以看到,模式是takeoff且是四旋翼的时候才会进入这个if语句里,在最后我用
home_position_valid
来判断是不是使用的gps,不是的话,我就判断当前的高度与期望的高度差,满足阈值的话,就认为到达了高度takeoff之后
takeoff完成了之后,飞行器会切换到loiter模式,但是我们使用的是uart的数据,没有gps,所以我们必须进行修改,一: 到达高度之后切换模式为position。二:修改loiter的数据
我选择修改loiter的使用数据:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55void
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
} else {
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if(_navigator->home_position_valid()){
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
} else {
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
item->altitude = _navigator->get_home_position()->alt + min_clearance;
}
}
}
else {
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
item->x = pos_sp_triplet->current.x;
item->y = pos_sp_triplet->current.y;
item->altitude = pos_sp_triplet->current.alt;
} else {
/* use current position and use return altitude as clearance */
item->x = _navigator->get_local_position()->x;
item->y = _navigator->get_local_position()->y;
item->altitude = -_navigator->get_local_position()->z;
}
}
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
}