//Function that converts target wp from float point versions to int
bool_t mission_element_convert(struct _mission_element *el)
{
struct _mission_element tmp_element = *el;
uint8_t i = 0;
switch (tmp_element.type) {
case MissionWP:
ENU_BFP_OF_REAL(el->element.mission_wp.wp.wp_i, tmp_element.element.mission_wp.wp.wp_f);
break;
case MissionCircle:
ENU_BFP_OF_REAL(el->element.mission_circle.center.center_i, tmp_element.element.mission_circle.center.center_f);
break;
case MissionSegment:
ENU_BFP_OF_REAL(el->element.mission_segment.from.from_i, tmp_element.element.mission_segment.from.from_f);
ENU_BFP_OF_REAL(el->element.mission_segment.to.to_i, tmp_element.element.mission_segment.to.to_f);
break;
case MissionPath:
for (i = 0; i < 5; i++) {
ENU_BFP_OF_REAL(el->element.mission_path.path.path_i[i], tmp_element.element.mission_path.path.path_f[i]);
}
break;
default:
// invalid element type
return FALSE;
break;
}
return TRUE;
}
void stateCalcSpeedEnu_i(void) {
if (bit_is_set(state.speed_status, SPEED_ENU_I))
return;
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.speed_status, SPEED_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
/* transform ecef_f -> ecef_i -> enu_i , set status bits */
SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
SetBit(state.speed_status, SPEED_ECEF_I);
enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
}
else { /* could not get this representation, set errno */
errno = 1;
}
}
else if (state.utm_initialized_f) {
if (bit_is_set(state.speed_status, SPEED_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
}
else { /* could not get this representation, set errno */
errno = 2;
}
}
else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct EnuCoor_i _enu_zero = {0};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_ENU_I);
}
bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
rectangle_survey_sweep_num = 0;
nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
survey_orientation = so;
if (survey_orientation == NS) {
if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
} else {
survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
grid = -grid;
}
if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else {
survey_from.y = nav_survey_south;
survey_to.y = nav_survey_north;
}
} else { /* survey_orientation == WE */
if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
} else {
survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
grid = -grid;
}
if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
} else {
survey_from.x = nav_survey_west;
survey_to.x = nav_survey_east;
}
}
nav_survey_shift = grid;
survey_uturn = FALSE;
nav_survey_rectangle_active = FALSE;
//go to start position
ENU_BFP_OF_REAL(survey_from_i, survey_from);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
VECT3_COPY(navigation_target, survey_from_i);
LINE_STOP_FUNCTION;
NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
if (survey_orientation == NS) {
nav_set_heading_deg(0);
} else {
nav_set_heading_deg(90);
}
return FALSE;
}
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_f = *enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
waypoint_globalize(wp_id);
}
}
/**
* Calculate LLA (int) from any other available representation.
* Note that since LLA in float has bad precision this is the last choice.
* So we mostly first convert to ECEF and then use lla_of_ecef_i
* which provides higher precision but is currently using the double function internally.
*/
void stateCalcPositionLla_i(void)
{
if (bit_is_set(state.pos_status, POS_LLA_I)) {
return;
}
if (bit_is_set(state.pos_status, POS_ECEF_I)) {
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> ecef_i -> lla_i, set status bits */
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
/* transform ned_i -> ecef_i -> lla_i, set status bits */
ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
/* transform enu_i -> ecef_i -> lla_i, set status bits */
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
/* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
/* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> lla_f -> lla_i, set status bits */
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
} else {
/* could not get this representation, set errno */
//struct LlaCoor_i _lla_zero = {0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_I);
}
bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
{
static bool_t is_last_half = FALSE;
static float survey_radius;
nav_survey_active = TRUE;
/* entry scan */ // wait for start position and altitude be reached
if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
|| (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
} else {
if (!nav_survey_rectangle_active) {
nav_survey_rectangle_active = TRUE;
LINE_START_FUNCTION;
}
nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
/* Update the current segment from corners' coordinates*/
if (SurveyGoingNorth()) {
survey_to.y = nav_survey_north;
survey_from.y = nav_survey_south;
} else if (SurveyGoingSouth()) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else if (SurveyGoingEast()) {
survey_to.x = nav_survey_east;
survey_from.x = nav_survey_west;
} else if (SurveyGoingWest()) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
}
if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
/* if you like to use position croos instead of approaching uncoment this line
if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
(stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
*/
/* Continue ... */
ENU_BFP_OF_REAL(survey_to_i, survey_to);
if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
ENU_BFP_OF_REAL(survey_from_i, survey_from);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
} else {
if (survey_orientation == NS) {
/* North or South limit reached, prepare turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if ((x0 + nav_survey_shift < nav_survey_west)
|| (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
|| ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
x0 = x0 + survey_radius; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = x0;
waypoints[0].enu_f.y = survey_from.y;
/* Computes the right direction */
if (SurveyGoingEast()) {
survey_radius = -survey_radius;
}
} else { /* (survey_orientation == WE) */
/* East or West limit reached, prepare turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0 + nav_survey_shift < nav_survey_south
|| my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
|| ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
my_y0 = my_y0 + survey_radius; /* latitude of next leg */
survey_from.y = survey_to.y = my_y0;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = survey_from.x;
waypoints[0].enu_f.y = my_y0;
/* Computes the right direction */
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
}
nav_in_segment = FALSE;
survey_uturn = TRUE;
LINE_STOP_FUNCTION;
#ifdef DIGITAL_CAM
float temp;
if (survey_orientation == NS) {
temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
} else{
temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
}
double inteiro;
double fract = modf (temp , &inteiro);
if (fract > .5) {
dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep
}
#endif
}
} else { /* START turn */
static struct EnuCoor_f temp_f;
if (survey_orientation == WE) {
temp_f.x = waypoints[0].enu_f.x;
temp_f.y = waypoints[0].enu_f.y - survey_radius;
} else {
temp_f.y = waypoints[0].enu_f.y;
temp_f.x = waypoints[0].enu_f.x - survey_radius;
}
//detect when segment has done
/* if you like to use position croos instead of approaching uncoment this line
if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
(stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
*/
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
if (nav_approaching_from(&survey_to_i, NULL, 0)) {
survey_uturn = FALSE;
nav_in_circle = FALSE;
LINE_START_FUNCTION;
} else {
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
}
} /* END turn */
} /* END entry scan */
return TRUE;
}// /* END survey_retangle */
void stateCalcPositionNed_i(void)
{
if (bit_is_set(state.pos_status, POS_NED_I)) {
return;
}
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> ned_f, set status bit, then convert to int */
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> ned_f -> ned_i, set status bits */
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct NedCoor_i _ned_zero = {0};
//return _ned_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_NED_I);
}
