void ahrs_update_mag_2d(void) {
struct FloatVect2 expected_ltp;
VECT2_COPY(expected_ltp, ahrs_impl.mag_h);
// normalize expected ltp in 2D (x,y)
FLOAT_VECT2_NORMALIZE(expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 measured_ltp;
FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);
struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y};
// normalize measured ltp in 2D (x,y)
FLOAT_VECT2_NORMALIZE(measured_ltp_2d);
const struct FloatVect3 residual_ltp =
{ 0,
0,
measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x };
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
*/
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
/**
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
float divider, angle_rad = angle / 180.0 * M_PI;
if (angle < 0.0) {
angle += 360.0;
}
if (angle >= 360.0) {
angle -= 360.0;
}
survey.poly_first = first_wp;
survey.poly_count = size;
survey.psa_sweep_width = sweep_width;
survey.psa_min_rad = min_rad;
survey.psa_shot_dist = shot_dist;
survey.psa_altitude = altitude;
survey.segment_angle = angle;
survey.return_angle = angle + 180;
if (survey.return_angle > 359) {
survey.return_angle -= 360;
}
if (angle <= 45.0 || angle >= 315.0) {
//north
survey.dir_vec.y = 1.0;
survey.dir_vec.x = 1.0 * tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
} else if (angle <= 135.0) {
//east
survey.dir_vec.x = 1.0;
survey.dir_vec.y = 1.0 / tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = survey.dir_vec.y / survey.dir_vec.x;
} else if (angle <= 225.0) {
//south
survey.dir_vec.y = -1.0;
survey.dir_vec.x = -1.0 * tanf(angle_rad);
sweep.x = -1.0;
sweep.y = survey.dir_vec.x / survey.dir_vec.y;
} else {
//west
survey.dir_vec.x = -1.0;
survey.dir_vec.y = -1.0 / tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
}
//normalize
FLOAT_VECT2_NORMALIZE(sweep);
VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);
//begin at leftmost position (relative to survey.dir_vec)
VECT2_COPY(small, waypoints[survey.poly_first]);
divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);
//calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
} else
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
//calculate the line the defines the first flyover
survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
survey.stage = ERR;
return;
}
//center of the entry circle
VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
survey.stage = ENTRY;
}
/**
* initializes the variables needed for the survey to start.
*
* @param center_wp the waypoint defining the center of the survey-rectangle
* @param dir_wp the waypoint defining the orientation of the survey-rectangle
* @param sweep_length the length of the survey-rectangle
* @param sweep_spacing distance between the sweeps
* @param sweep_lines number of sweep_lines to fly
* @param altitude the altitude that must be reached before the flyover starts
*/
bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{
zs.current_laps = 0;
zs.pre_leave_angle = 2;
// copy variables from the flight plan
VECT2_COPY(zs.wp_center, waypoints[center_wp]);
VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
zs.altitude = altitude;
// if turning right leave circle before angle is reached, if turning left - leave after
if (sweep_spacing > 0) {
zs.pre_leave_angle -= zs.pre_leave_angle;
}
struct FloatVect2 flight_vec;
VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
FLOAT_VECT2_NORMALIZE(flight_vec);
// calculate the flight_angle
zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
zs.return_angle = zs.flight_angle + 180;
if (zs.return_angle > 359) {
zs.return_angle -= 360;
}
// calculate the vector from one flightline perpendicular to the next flightline left,
// seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
// (used later to move the flight pattern one flightline up for each round)
zs.sweep_width.x = -flight_vec.y * sweep_spacing;
zs.sweep_width.y = +flight_vec.x * sweep_spacing;
// calculate number of laps to fly and turning radius for each end
zs.total_laps = (sweep_lines+1)/2;
zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
struct FloatVect2 flight_line;
VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
//CALCULATE THE NAVIGATION POINTS
//start and end of flight-line in flight-direction
VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
struct FloatVect2 sweep_span;
VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
//start and end of flight-line in return-direction
VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
//turn-centers at both ends
zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(100.0);
NavVerticalAltitudeMode(zs.altitude, 0.0);
zs.stage = Z_ENTRY;
return FALSE;
}
