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;
}
// 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;
}
// 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;
}