/** Reset the UTM zone to current GPS fix */
unit_t nav_reset_utm_zone(void) {
struct UtmCoor_f utm0_old;
utm0_old.zone = nav_utm_zone0;
utm0_old.north = nav_utm_north0;
utm0_old.east = nav_utm_east0;
utm0_old.alt = ground_alt;
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, &utm0_old);
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
nav_utm_zone0 = gps.utm_pos.zone;
#endif
struct UtmCoor_f utm0;
utm0.zone = nav_utm_zone0;
utm_of_lla_f(&utm0, &lla0);
nav_utm_east0 = utm0.east;
nav_utm_north0 = utm0.north;
stateSetLocalUtmOrigin_f(&utm0);
return 0;
}
void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) {
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
utm->zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
/**
* 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);
}
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
{
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm->zone = gps.utm_pos.zone;
}
else {
utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
}
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
void stateCalcPositionLla_i(void) {
if (bit_is_set(state.pos_status, POS_LLA_I))
return;
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_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 -> lla_f, set status bit, then convert to int */
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(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_NED_F)) {
/* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(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_NED_I)) {
/* transform ned_i -> ecef_i -> lla_i, set status bits */
/// @todo check if resolution is enough
ecef_of_ned_point_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); /* uses double version internally */
}
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);
}
void stateCalcPositionLla_f(void) {
if (bit_is_set(state.pos_status, POS_LLA_F))
return;
if (bit_is_set(state.pos_status, POS_LLA_I)) {
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
/* transform ecef_i -> ecef_f -> lla_f, set status bits */
ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_F)) {
/* transform ned_f -> ecef_f -> lla_f, set status bits */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_I)) {
/* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_UTM_F)) {
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
}
else {
/* could not get this representation, set errno */
//struct LlaCoor_f _lla_zero = {0.0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_F);
}
/** Convert a local NED position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED position in meter << #INT32_POS_FRAC
*/
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct UtmCoor_f utm_f;
UTM_FLOAT_OF_BFP(utm_f, *utm);
/* calls the floating point transformation */
struct LlaCoor_f lla_f;
lla_of_utm_f(&lla_f, &utm_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_f);
#else // use double precision by default
/* convert our input to floating point */
struct UtmCoor_d utm_d;
UTM_DOUBLE_OF_BFP(utm_d, *utm);
/* calls the floating point transformation */
struct LlaCoor_d lla_d;
lla_of_utm_d(&lla_d, &utm_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_d);
#endif
}
/*******************************************************************
; function name: vPoint
; description: Transforms ground coordinate system into
; plane's coordinate system via three rotations
; and determines positions of camera servos.
; parameters: fPlaneNorth, fPlaneEast, fPlaneAltitude plane's
; position with respect to ground
; in m (actually the units do not matter as
; long as they are the same as for the object's
; position)
; fRollAngle level=0; right wing down = positive values
; fPitchAngle level=0; nose up = positive values
; plane's pitch and roll angles
; with respect to ground in radians
; fYawAngle north=0; right= positive values in radians
; plane's yaw angle with respect to north
; fObjectNorth, fObjectEast, fAltitude object's
; position with respect to ground
; in m (actually the units do not matter as
; long as they are the same for the plane's
; position)
; fPan, fTilt angles for camera servos in radians,
; pan is turn/left-right and tilt is down-up
; in reference to the camera picture
; camera mount: The way the camera is mounted is given through a
; define POINT_CAM_a_[_b] where a gives the mount
; angle within the aircraft and b the angle when
; viewing the direction of the first servo.
;*******************************************************************/
void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
float fRollAngle, float fPitchAngle, float fYawAngle,
float fObjectEast, float fObjectNorth, float fAltitude,
float *fPan, float *fTilt)
{
static VECTOR svPlanePosition,
svObjectPosition,
svObjectPositionForPlane,
svObjectPositionForPlane2;
static VECTOR sv_cam_projection,
sv_cam_projection_buf;
static MATRIX smRotation;
/*** BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS) ***/
/*** IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT ***/
/*** THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC. ***/
/*** IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED ***/
if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET || cam_mode == CAM_MODE_XY_TARGET ){
if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){
/*######################################## TILT CONTROL INPUT #############################################*/
#ifdef CAM_TILT_NEUTRAL
#if defined(RADIO_TILT)
if ((*fbw_state).channels[RADIO_TILT] >= 0){
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
}else{
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MIN_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
}
#elif defined(RADIO_PITCH)
if ((*fbw_state).channels[RADIO_PITCH] >= 0){
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
}else{
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MIN_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
}
#else
#error RADIO_TILT or RADIO_PITCH not defined.
#endif
#else //#ifdef CAM_TILT_NEUTRAL
#if defined(RADIO_TILT)
cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ));
#elif defined(RADIO_PITCH)
cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ));
#else
#error RADIO_TILT or RADIO_PITCH not defined.
#endif
#endif //#ifdef CAM_TILT_NEUTRAL
/*######################################## END OF TILT CONTROL INPUT ########################################*/
/*########################################### PAN CONTROL INPUT #############################################*/
#ifdef CAM_PAN_NEUTRAL
#if defined(RADIO_PAN)
if ((*fbw_state).channels[RADIO_PAN] >= 0){
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ) *
RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
}else{
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MIN_PPRZ) *
RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
}
#elif defined(RADIO_ROLL)
if ((*fbw_state).channels[RADIO_ROLL] >= 0){
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ) *
RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
}else{
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MIN_PPRZ) *
RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
}
#else
#error RADIO_PAN or RADIO_ROLL not defined.
#endif
#else //#ifdef CAM_PAN_NEUTRAL
#if defined(RADIO_PAN)
cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ));
#elif defined(RADIO_ROLL)
cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ));
#else
#error RADIO_PAN or RADIO_ROLL not defined.
#endif
#endif //#ifdef CAM_PAN_NEUTRAL
/*######################################## END OF PAN CONTROL INPUT #############################################*/
// Bound Pan and Tilt angles.
if (cam_theta > RadOfDeg(CAM_TILT_MAX)){
cam_theta = RadOfDeg(CAM_TILT_MAX);
}else if(cam_theta < RadOfDeg(CAM_TILT_MIN)){ cam_theta = RadOfDeg(CAM_TILT_MIN); }
if (cam_phi > RadOfDeg(CAM_PAN_MAX)){
cam_phi = RadOfDeg(CAM_PAN_MAX);
}else if(cam_phi < RadOfDeg(CAM_PAN_MIN)){ cam_phi = RadOfDeg(CAM_PAN_MIN); }
svPlanePosition.fx = fPlaneEast;
svPlanePosition.fy = fPlaneNorth;
svPlanePosition.fz = fPlaneAltitude;
// FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
//cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
//cam_theta = RadOfDeg(70); // LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
if ( cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC){ // Not much to see after 80 degrees of tilt so stop tracking.
*fPan = cam_phi;
*fTilt = cam_theta;
#ifdef SHOW_CAM_COORDINATES
cam_point_distance_from_home = 0;
cam_point_lon = 0;
cam_point_lat = 0;
#endif
return;
}else{
sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta)*(fPlaneAltitude-ground_alt));
sv_cam_projection_buf.fy = svPlanePosition.fy;
}
#if defined(WP_CAM_POINT)
sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
#else
sv_cam_projection_buf.fz = ground_alt;
#endif
/* distance between plane and camera projection */
vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
if(heading_radians > RadOfDeg(180)){ heading_radians -= RadOfDeg(360); }
if(heading_radians < RadOfDeg(-180)){ heading_radians += RadOfDeg(360); }
//heading_radians += cam_theta;
/* camera pan angle correction, using a clockwise rotation */
smRotation.fx1 = (float)(cos(cam_phi));
smRotation.fx2 = (float)(sin(cam_phi));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
/* yaw correction using a counter clockwise rotation*/
smRotation.fx1 = (float)(cos(heading_radians));
smRotation.fx2 = -(float)(sin(heading_radians));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
#if defined(RADIO_CAM_LOCK)
if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ/2)) && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = TRUE; }
if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ/2 && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = FALSE; }
#endif
// When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
if (cam_lock == FALSE){
fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
fAltitude = ground_alt;
memory_x = fObjectEast;
memory_y = fObjectNorth;
memory_z = fAltitude;
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = ground_alt;
#endif
#if defined(SHOW_CAM_COORDINATES)
cam_point_x = fObjectEast;
cam_point_y = fObjectNorth;
cam_point_distance_from_home = distance_correction * (uint16_t) (sqrt((cam_point_x*cam_point_x) + (cam_point_y*cam_point_y) ));
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east/100. + sv_cam_projection.fx;
utm.north = gps.utm_pos.north/100. + sv_cam_projection.fy;
utm.zone = gps.utm_pos.zone;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon*(180/M_PI);
cam_point_lat = lla.lat*(180/M_PI);
#endif
}else{
fObjectEast = memory_x;
fObjectNorth = memory_y;
fAltitude = memory_z;
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
#endif
}
if (cam_mode == CAM_MODE_RC && cam_lock == 0 ){
*fPan = cam_phi;
*fTilt = cam_theta;
return;
}
#if defined(WP_CAM_POINT)
else{
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
}
#endif
/*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/
}else{
/*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/
#ifdef SHOW_CAM_COORDINATES
cam_point_distance_from_home = distance_correction * (uint16_t) fabs( ((uint16_t) (sqrt((fObjectNorth*fObjectNorth) + (fObjectEast*fObjectEast) ))) -
((uint16_t) (sqrt((fPlaneNorth*fPlaneNorth) + (fPlaneEast*fPlaneEast) ))) );
struct UtmCoor_f utm;
utm.east = nav_utm_east0 + fObjectEast;
utm.north = nav_utm_north0 + fObjectNorth;
utm.zone = gps.utm_pos.zone;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon*(180/M_PI);
cam_point_lat = lla.lat*(180/M_PI);
#endif
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
#endif
}
}
