void hoverPitchCntrl(void)
{
    int16_t max_tilt_sine = sine((int8_t)(MAX_TILT*.7111));

    if (flags._.pitch_feedback && flags._.GPS_steering)
	{
        //error along y axis between aircraft position and goal (origin point here) in cm
#ifdef TestGPSPositioning
        hovering_pitch_order = RMAX;
        tofinish_line_factor10 = 30;
#endif
        if (control_position_hold)
        {
            target_pitch = compute_target_pitch(hovering_pitch_order, tofinish_line_factor10, max_tilt_sine);
        }
        else
        {
            target_pitch = 0;
            pitch_error_integral = 0;
            pitch_v_error_integral = 0;
        }

        //additional_int16_export4 = target_pitch;
        
        uint16_t horizontal_air_speed = vector2_mag(IMUvelocityx._.W1 - estimatedWind[0], 
	                                   IMUvelocityy._.W1 - estimatedWind[1]);
        
#ifdef TestGPSPositioning
        horizontal_air_speed = 0;
#endif
        
		//PI controller on pitch angle
		pitch_v_target = compute_pi_block(-rmat[7], -target_pitch, hoverpitchToWPkp, hoverpitchToWPki, &pitch_error_integral, 
                                    (int16_t)(SERVO_HZ), limitintegralpitchToWP, control_position_hold);
        
        //additional_int16_export3 = -rmat[7];
                
        pitch_v_target = limit_value(pitch_v_target, -limittargetpitchV, limittargetpitchV);
        
        pitch_hover_corr = compute_pi_block(horizontal_air_speed, pitch_v_target, hoverpitchToWPvkp, 0, &pitch_v_error_integral, 
                                  (int16_t)(SERVO_HZ), limitintegralpitchVToWP, control_position_hold);
        
        //additional_int16_export8 = pitch_hover_corr;
	}
	else
	{
		pitch_hover_corr = 0;
	}

	pitch_control = pitch_hover_corr;
}
Beispiel #2
0
// Match points for a single scan
void rmap_match_scan(rmap_t *self, rmap_scan_t *scan_a)
{
  int i;
  int ni, nj, di, dj, mi, mj;
  double d;
  rmap_constraint_t *con;
  rmap_constraint_t **cons;
  rmap_cell_t *cell;
  vector2_t pa, pb;
  vector2_t qa, qb;
  rmap_item_t *item;
  rmap_scan_t *scan_b;

  // Workspace for constraint map
  printf("num-scans = %d\n",self->num_scans);
  cons = new rmap_constraint_t *[self->num_scans];
  
  // Match hits to boundaries
  for (i = 0; i < scan_a->num_hits; i += self->range_interval)
  {
    pa = scan_a->hits[i];
    qa = pose2_add_pos(pa, scan_a->pose);      

    ni = RMAP_GRIDX(self, qa.x);
    nj = RMAP_GRIDY(self, qa.y);

    // Reset the map from scan index to constraint pointer
    memset(cons, 0, self->num_scans * sizeof(cons[0]));

    // Look in the grid for the nearest boundary point; we have to
    // check all the cells in the vicinity of the hit point.
    for (dj = -1; dj <= +1; dj++)
    {
      for (di = -1; di <= +1; di++)
      {
        mi = ni + di;
        mj = nj + dj;
        if (RMAP_GRID_VALID(self, mi, mj))
        {
          cell = self->grid + RMAP_GRID_INDEX(self, mi, mj);

          for (item = cell->first; item != NULL; item = item->next)
          {
            scan_b = item->scan;
            if (scan_b == scan_a)
              continue;

            pb = item->local;
            qb = item->global;
            
            d = vector2_mag(vector2_sub(qa, qb));

            con = cons[scan_b->index];
            if (con == NULL)
            {
              assert(self->num_cons < self->max_cons);
              con = self->cons + self->num_cons++;
              cons[scan_b->index] = con;              
              con->scan_a = scan_a;
              con->scan_b = scan_b;
              con->local_a = pa;
              con->dist = DBL_MAX;
            }
    
            if (d < con->dist)
            {
              assert(con->scan_b == scan_b);
              con->local_b = pb;
              con->dist = d;
            }
          }
        }
      }
    }
  }

  delete [] cons;
    
  return;
}
Beispiel #3
0
// Find correction
double lodo_correct(lodo_t *self)
{
  int i, ni, erc;
  vector2_t q;
  double r;
  double interval;
  double offset, err;
  double best_offset, best_err, best_outliers;
  gsl_function func;

  // Pre-compute some values used in the test function
  for (i = 0; i < self->num_ranges; i++)
  {
    // Get range relative to laser
    r = self->scan.ranges[i];
    if (r > self->range_max)
    {
      self->scan_points[i].ni = -1;
      continue;
    }

    // Compute cartesian points relative to robot
    q.x = r * cos(i * self->range_step + self->range_start);
    q.y = r * sin(i * self->range_step + self->range_start);    //printf("ni=%d\n",ni);

    q = pose2_add_pos(q, self->laser_pose);

    // Compute range relative to robot
    r = vector2_mag(q);
    ni = LODO_LTOR(r);
	//printf ("ni = %d\tself->pef_num_ranges = %d\n", ni, self->pef_num_ranges);
    if (ni < 0 || ni > self->pef_num_ranges)
      ni = -1;
    self->scan_points[i].ni = ni;
  }

  best_offset = 0.0;
  best_err = DBL_MAX;

  // Initialize the minimizer
  func.function = (double (*) (double, void*)) lodo_correct_func;
  func.params = self;
  erc = gsl_min_fminimizer_set(self->mini, &func,
                               0.0, -self->fit_interval, +self->fit_interval);

  // If the minimizer failes, revert to exhaustive search
  if (erc != GSL_SUCCESS)
  {
    //printf("brute force\n\n");
    best_err = DBL_MAX;
    for (i = -100; i <= 100; i++)
    {
      offset = i * self->fit_interval / 100.0;
      err = lodo_test_offset(self, offset, NULL);
      //printf("%d %f %f\n", i, offset, err);
      if (err < best_err)
      {
        best_err = err;
        best_offset = offset;
      }
    }
    //printf("\n\n");
  }
  else
  {
    for (i = 0; i < 10; i++) // HACK
    {
      erc = gsl_min_fminimizer_iterate(self->mini);
      if (erc == GSL_EBADFUNC)
        assert(0);
      if (erc == GSL_FAILURE)
        break;

      // Test for convergence
      interval = gsl_min_fminimizer_x_upper(self->mini);
      interval -= gsl_min_fminimizer_x_lower(self->mini);
      if (interval < 0.01 * M_PI / 180) // HACK
        break;
    }

    best_offset = gsl_min_fminimizer_x_minimum(self->mini);
    best_err = gsl_min_fminimizer_f_minimum(self->mini);
  }

  // Re-do the test to get the outlier count
  lodo_test_offset(self, best_offset, &best_outliers);

  // Check it his is a good fit.
  if (best_err > self->fit_err_thresh)
  {
    self->fit_valid = 0;
    self->fit_add = 0;
    self->fit_correct = 0;
    return 0;
  }

  self->fit_valid = 1;

  // See if should add this scan to the map.  The logic is this: if we
  // have a good fit, but there are lots of points that are "outliers"
  // (i.e., large-ish error value), then these points should be added
  // to the map.  Works for both turning in place and "bursting"
  // through doorways.
  if (fabs(best_offset) < 5 * M_PI / 180) // HACK
  {
    if (best_outliers > self->fit_outlier_frac)
      self->fit_add = 1;
    else
      self->fit_add = 0;
  }

  // Correct the scan pose
  self->fit_correct = best_offset;
  self->scan.cpose.rot = pose2_add_rot(self->fit_correct, self->scan.cpose);

  return 0;
}