/// Utility function: converts lla (int) to local point (float)
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
// return FALSE if there is no valid local coordinate system
if (!state.ned_initialized_i) {
return FALSE;
}
// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_i tmp_enu_point_i;
enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla);
struct EnuCoor_f tmp_enu_point_f;
ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i);
//Bound the new waypoint with max distance from home
struct EnuCoor_f home;
ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
point->z = tmp_enu_point_f.z;
return TRUE;
}
/** update local ENU coordinates from its LLA coordinates */
void waypoint_localize(uint8_t wp_id)
{
if (state.ned_initialized_i) {
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
// convert ENU pos from cm to BFP with INT32_POS_FRAC
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;
waypoints[wp_id].enu_i = enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
}
}
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);
}
void stateCalcPositionEnu_i(void)
{
if (bit_is_set(state.pos_status, POS_ENU_I)) {
return;
}
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.pos_status, POS_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_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);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> enu_f, set status bit, then convert to int */
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> ecef_f -> enu_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);
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
enu_of_lla_point_i(&state.enu_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_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> enu_f -> enu_i , set status bits */
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_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);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_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 EnuCoor_i _enu_zero = {0};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ENU_I);
}
