使用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准备position_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)
item->lat = _navigator->get_local_position()->x; item->lon = _navigator->get_local_position()->y;
将经纬度的变量赋值给位置期望
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)
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->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;
position_control.cpp
因为takeoff原本使用的是gps的数据,在位置控制的时候,px4将其进行坐标转化,转化为机体坐标系的数据,在navigator中我们使用的就是机体坐标系的xy数据,所以不需要进行坐标系的转化。
坐标系转化://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; }
改动:
//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模式无关。if ((_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的使用数据:
void 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; } }