使用uart数据起飞

使用uart得到的位置信息进行起飞

在得到了位置信息的前提下，我们开始进行模拟起飞，即使用usb供电，人工控制其高度，在上位机查看油门大小，电机的pwm输出。

1. commander.cpp
   在commander.cpp中，主要是判断能不能切换到takeoff模式，并且设置一些标志位，为takeoff做准备

2. navigator_main.cpp
   在navigator中，只要是通过run()重载，来确定使用的函数，takeoff模式下当然是进入takeoff.cpp。

   1. 在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;

   2. 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;

3. 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;
       }

4. 模拟飞行
   解锁之后应该能看到油门数据突变，变为油门悬停值，当你升高飞行器至起飞高度后，模式切换为loier （待添加图片）

5. 起飞之后

   起飞之后在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，不是的话，我就判断当前的高度与期望的高度差，满足阈值的话，就认为到达了高度

6. 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;
       }
   }