void stateCalcHorizontalSpeedNorm_f(void) {
if (bit_is_set(state.speed_status, SPEED_HNORM_F))
return;
if (bit_is_set(state.speed_status, SPEED_HNORM_I)){
state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.speed_status, SPEED_NED_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
SetBit(state.speed_status, SPEED_ENU_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_HNORM_F);
}
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;
}
static void handle_ins_msg(void)
{
update_state_interface();
if (xsens.new_attitude) {
new_ins_attitude = true;
xsens.new_attitude = false;
}
#if USE_GPS_XSENS
if (xsens.gps_available) {
// Horizontal speed
float fspeed = FLOAT_VECT2_NORM(xsens.vel);
if (xsens.gps.fix != GPS_FIX_3D) {
fspeed = 0;
}
xsens.gps.gspeed = fspeed * 100.;
xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;
float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
xsens.gps.course = fcourse * 1e7;
SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);
gps_xsens_publish();
xsens.gps_available = false;
}
#endif // USE_GPS_XSENS
}
void stateCalcHorizontalSpeedNorm_i(void) {
if (bit_is_set(state.speed_status, SPEED_HNORM_I))
return;
if (bit_is_set(state.speed_status, SPEED_HNORM_F)){
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
/// @todo consider INT32_SPEED_FRAC
INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
/// @todo consider INT32_SPEED_FRAC
INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
/* transform ecef speed to ned, set status bit, then compute norm */
//foo
/// @todo consider INT32_SPEED_FRAC
//INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
SetBit(state.speed_status, SPEED_NED_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else {
//int32_t _norm_zero = 0;
//return _norm_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_HNORM_I);
}
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
{
nps_atmosphere.wind.x = wind_north;
nps_atmosphere.wind.y = wind_east;
nps_atmosphere.wind.z = wind_down;
/* recalc horizontal wind speed and dir */
nps_atmosphere.wind_speed = FLOAT_VECT2_NORM(nps_atmosphere.wind);
double dir = atan2(-wind_east, -wind_north);
/* normalize dir to 0-2Pi */
while (dir < 0.0) {
dir += 2 * M_PI;
}
while (dir >= 2 * M_PI) {
dir -= 2 * M_PI;
}
nps_atmosphere.wind_dir = dir;
}
bool_t nav_spiral_run(void)
{
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.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);
FLOAT_VECT2_NORM(DistanceStartEstim, 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;
}
