static void guidance_h_update_reference(void)
{
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF
if (bit_is_set(guidance_h.sp.mask, 5)) {
gh_update_ref_from_speed_sp(guidance_h.sp.speed);
} else {
gh_update_ref_from_pos_sp(guidance_h.sp.pos);
}
#endif
/* either use the reference or simply copy the pos setpoint */
if (guidance_h.use_ref) {
/* convert our reference to generic representation */
INT32_VECT2_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h.ref.speed, gh_ref.speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(guidance_h.ref.accel, gh_ref.accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
} else {
VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
}
#if GUIDANCE_H_USE_SPEED_REF
if (guidance_h.mode == GUIDANCE_H_MODE_HOVER) {
VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
}
#endif
/* update heading setpoint from rate */
if (bit_is_set(guidance_h.sp.mask, 7)) {
guidance_h.sp.heading += (guidance_h.sp.heading_rate >> (INT32_ANGLE_FRAC - INT32_RATE_FRAC)) / PERIODIC_FREQUENCY;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
}
void ins_update_gps(void) {
#ifdef USE_GPS
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
if (!ins_ltp_initialised) {
ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
ins_ltp_def.hmsl = booz_gps_state.hmsl;
ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
if (ins_hf_realign) {
ins_hf_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#ifndef USE_VFF /* vff not used */
ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
#endif /* vff not used */
#endif /* hff used */
#ifndef USE_HFF /* hff not used */
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#else /* only vff used */
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif
#ifdef USE_GPS_LAG_HACK
VECT2_COPY(d_pos, ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(ins_ltp_pos, d_pos);
#endif
#endif /* hff not used */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
#endif /* USE_GPS */
}
static inline void reset_guidance_reference_from_current_position(void)
{
VECT2_COPY(guidance_h.ref.pos, *stateGetPositionNed_i());
VECT2_COPY(guidance_h.ref.speed, *stateGetSpeedNed_i());
INT_VECT2_ZERO(guidance_h.ref.accel);
gh_set_ref(guidance_h.ref.pos, guidance_h.ref.speed, guidance_h.ref.accel);
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
{
VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix
nav_spiral.center.z = waypoints[center_wp].a;
nav_spiral.radius_start = radius_start; // start radius of the helix
nav_spiral.segments = segments;
nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
if (nav_spiral.radius_start < nav_spiral.radius_min)
nav_spiral.radius_start = nav_spiral.radius_min;
nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
struct FloatVect2 edge;
VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
FLOAT_VECT2_NORM(nav_spiral.radius, edge);
// get a copy of the current position
struct EnuCoor_f pos_enu;
memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
// nav_spiral.alpha_limit denotes angle, where the radius will be increased
nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
//current position
nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
if(nav_spiral.dist_from_center > nav_spiral.radius)
nav_spiral.status = SpiralOutside;
return FALSE;
}
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
{
struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
VECT2_DIFF(wp_diff, *wp_end, *wp_start);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
// go back to metric precision or values are too large
VECT2_COPY(wp_diff_prec, wp_diff);
INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
nav_leg_length = int32_sqrt(leg_length2);
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
int32_t prog_2 = nav_leg_length;
Bound(nav_leg_progress, 0, prog_2);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
VECT2_SUM(navigation_target, *wp_start, progress_pos);
nav_segment_start = *wp_start;
nav_segment_end = *wp_end;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
dist2_to_wp = get_dist2_to_point(wp_end);
}
void nav_run(void) {
#if GUIDANCE_H_USE_REF
// if GUIDANCE_H_USE_REF, CARROT_DIST is not used
VECT2_COPY(navigation_carrot, navigation_target);
#else
nav_advance_carrot();
#endif
nav_set_altitude();
}
void ins_update_gps(void) {
#if USE_GPS
if (gps.fix == GPS_FIX_3D) {
if (!ins_ltp_initialised) {
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
ins_ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl;
ins_ltp_initialised = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def);
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
#if USE_HFF
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
if (ins.hf_realign) {
ins.hf_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#endif /* hff used */
#if !USE_HFF /* hff not used */
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* hff not used */
}
#endif /* USE_GPS */
}
void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
if (radius == 0) {
VECT2_COPY(navigation_target, *wp_center);
dist2_to_wp = get_dist2_to_point(wp_center);
}
else {
struct Int32Vect2 pos_diff;
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
int32_t last_qdr = nav_circle_qdr;
// compute qdr
nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
// increment circle radians
if (nav_circle_radians != 0) {
int32_t angle_diff = nav_circle_qdr - last_qdr;
INT32_ANGLE_NORMALIZE(angle_diff);
nav_circle_radians += angle_diff;
}
else {
// Smallest angle to increment at next step
nav_circle_radians = 1;
}
// direction of rotation
int8_t sign_radius = radius > 0 ? 1 : -1;
// absolute radius
int32_t abs_radius = abs(radius);
// carrot_angle
int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
int32_t s_carrot, c_carrot;
PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
PPRZ_ITRIG_COS(c_carrot, carrot_angle);
// compute setpoint
VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
VECT2_SUM(navigation_target, *wp_center, pos_diff);
}
nav_circle_center = *wp_center;
nav_circle_radius = radius;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
}
void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt)
{
struct FloatVect2 expected_ltp;
VECT2_COPY(expected_ltp, ahrs_fc.mag_h);
// normalize expected ltp in 2D (x,y)
float_vect2_normalize(&expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, *mag);
struct FloatVect3 measured_ltp;
float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.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);
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_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
* with ahrs_fc.mag_cnt beeing the number of propagations since last update
*/
const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
}
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);
}
static inline void UNUSED nav_advance_carrot(void)
{
struct EnuCoor_i *pos = stateGetPositionEnu_i();
/* compute a vector to the waypoint */
struct Int32Vect2 path_to_waypoint;
VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
/* saturate it */
VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY(navigation_carrot, navigation_target);
} else {
struct Int32Vect2 path_to_carrot;
VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
}
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING:
{
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING :
{
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING :
{
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
/* this case for DL_HOVER_POINT_CHANGE_ECEF is added by Yi and Edward */
/*********************************************************************/
case DL_HOVER_POINT_CHANGE_ECEF:
{
uint8_t ac_id = DL_HOVER_POINT_CHANGE_ECEF_ac_id(dl_buffer);
if(ac_id != AC_ID) break;
uint8_t external_gps_fixed = DL_HOVER_POINT_CHANGE_ECEF_external_gps_fix(dl_buffer);
/** getting the ecef coordinate from external gps **/
struct EcefCoor_i external_gps_ecef_pos;
external_gps_ecef_pos.x = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x(dl_buffer);//DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x() is defined in dl_protocol.h
external_gps_ecef_pos.y = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_y(dl_buffer);
external_gps_ecef_pos.z = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_z(dl_buffer);
DOWNLINK_SEND_HOVER_POINT_CHANGE_ECEF(DefaultChannel, DefaultDevice, &ac_id, &external_gps_fixed, &(external_gps_ecef_pos.x), &(external_gps_ecef_pos.y), &(external_gps_ecef_pos.z));
if(external_gps_fixed == 0x03)//case for 3D_Fix.
{
if(!ins_ltp_initialised || guidance_h_mode != GUIDANCE_H_MODE_HOVER) break;//ins_ltp_initialised defined in ins_int.h
//no HMOE reference
//not in the HOVER_Z_HOLD mode
//not in flight(looks like no needed)
struct NedCoor_i external_gps_ned_pos;
memset((void *) &external_gps_ned_pos,0,sizeof(struct NedCoor_i));//just in case, clear the memory.
/** convert the ecef coordinate into ned(ltp) coordinate **/
ned_of_ecef_point_i(&external_gps_ned_pos,&ins_ltp_def,&external_gps_ecef_pos);
/** change the current setpoint with external_gps_ned_pos coordinate **/
struct Int32Vect2 external_gps_h_pos;
//cm to m and store in external_gps_h_pos
//check function ins_update_gps() for reference.
INT32_VECT2_SCALE_2(external_gps_h_pos,external_gps_ned_pos,INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
//subsitute the guidance_h_pos_sp
VECT2_COPY(guidance_h_pos_sp,external_gps_h_pos);
/*
* the above code is used for congruity, but waste space and time.
* simple using
*
* guidance_h_pos_sp.x = external_gps_ned_pos.x;
* guidance_h_pos_sp.y = external_gps_ned_pox.y;
*
* can achieve the same effect.
*/
}
else break;//hover pos_sp no change
}
/**------------------------------------------------------------------*/
#if defined USE_NAVIGATION
case DL_BLOCK :
{
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP :
{
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) break;
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt is in cm, lla.alt in mm */
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
bool nav_spiral_run(void)
{
struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
float DistanceStartEstim;
float CircleAlpha;
switch (nav_spiral.status) {
case SpiralOutside:
//flys until center of the helix is reached an start helix
nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
// center reached?
if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
#endif
nav_spiral.status = SpiralStartCircle;
}
break;
case SpiralStartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to SpiralCircle
nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
if (nav_spiral.dist_from_center >= nav_spiral.radius_start) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = SpiralCircle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360 / nav_spiral.segments);
#endif
}
break;
case SpiralCircle: {
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
DistanceStartEstim = float_vect2_norm(&pos_diff);
CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start)));
if (CircleAlpha >= nav_spiral.alpha_limit) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = SpiralInc;
}
break;
}
case SpiralInc:
// increasing circle radius as long as it is smaller than max helix radius
if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) {
nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI;
}
#endif
} else {
nav_spiral.radius_start = nav_spiral.radius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
nav_spiral.status = SpiralCircle;
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
return true;
}
/**
* 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;
}
void dc_periodic_4Hz(void)
{
static uint8_t dc_shutter_timer = 0;
switch (dc_autoshoot) {
case DC_AUTOSHOOT_PERIODIC:
if (dc_shutter_timer) {
dc_shutter_timer--;
} else {
dc_shutter_timer = dc_autoshoot_quartersec_period;
dc_send_command(DC_SHOOT);
}
break;
case DC_AUTOSHOOT_DISTANCE:
{
struct FloatVect2 cur_pos;
cur_pos.x = stateGetPositionEnu_f()->x;
cur_pos.y = stateGetPositionEnu_f()->y;
struct FloatVect2 delta_pos;
VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
float dist_to_last_shot = float_vect2_norm(&delta_pos);
if (dist_to_last_shot > dc_distance_interval) {
dc_gps_count++;
dc_send_command(DC_SHOOT);
VECT2_COPY(last_shot_pos, cur_pos);
}
}
break;
case DC_AUTOSHOOT_CIRCLE:
{
float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
if (course < 0.)
course += 360.;
float current_block = floorf(course/dc_circle_interval);
if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
dc_gps_count++;
dc_circle_last_block = current_block;
dc_send_command(DC_SHOOT);
}
}
break;
case DC_AUTOSHOOT_SURVEY:
{
float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
dc_gps_next_dist += dc_survey_interval;
dc_gps_count++;
dc_send_command(DC_SHOOT);
}
}
break;
default:
dc_autoshoot = DC_AUTOSHOOT_STOP;
}
}
