/// 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); }