void ins_update_gps(void) {
if (gps_state.fix == GPS_FIX_3D) {
if (!ins.ltp_initialised) {
ltp_def_from_ecef_i(&ins.ltp_def, &gps_state.ecef_pos);
ins.ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&ins.gps_pos_cm_ned, &ins.ltp_def, &gps_state.ecef_pos);
ned_of_ecef_vect_i(&ins.gps_speed_cm_s_ned, &ins.ltp_def, &gps_state.ecef_speed);
#ifdef USE_HFF
b2ins_update_gps();
VECT2_SDIV(ins.ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp);
VECT2_SDIV(ins.ltp_speed, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_speed_ltp);
#else
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, ins.gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, ins.gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
VECT3_COPY(ins.ltp_pos, b2ins_meas_gps_pos_ned);
VECT3_COPY(ins.ltp_speed, b2ins_meas_gps_speed_ned);
#endif
INT32_VECT3_ENU_OF_NED(ins.enu_pos, ins.ltp_pos);
INT32_VECT3_ENU_OF_NED(ins.enu_speed, ins.ltp_speed);
INT32_VECT3_ENU_OF_NED(ins.enu_accel, ins.ltp_accel);
}
}
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
{
/* copy velocity from GPS */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
/* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_s->ned_vel,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
}
else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
/* convert ECEF to NED */
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
/* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
}
/* copy position from GPS */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
/* convert ECEF to NED */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_s->ecef_pos);
/* convert pos from cm to m in BFP with INT32_POS_FRAC */
INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
}
}
void ins_update_gps(void) {
#if USE_GPS
//Check for GPS fix
if (gps.fix == GPS_FIX_3D) {
//Set the initial coordinates
if(!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
//Set the x and y and maybe z position in ltp and save
struct NedCoor_i ins_gps_pos_cm_ned;
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
//When we don't want to use the height of the navdata we can use the gps height
#if USE_GPS_HEIGHT
INT32_VECT3_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#else
INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#endif
//Set the local origin
stateSetPositionNed_i(&ins_impl.ltp_pos);
}
#endif /* USE_GPS */
}
void ins_reset_altitude_ref(void)
{
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_gp.ltp_def, &lla);
ins_gp.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_gp.ltp_def);
}
#include "subsystems/abi.h"
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
if (gps_s->fix == GPS_FIX_3D) {
if (!ins_gp.ltp_initialized) {
ins_reset_local_origin();
}
/* simply scale and copy pos/speed from gps */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
stateSetPositionNed_i(&ins_gp.ltp_pos);
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
stateSetSpeedNed_i(&ins_gp.ltp_speed);
}
}
void ins_gps_passthrough_register(void);
{
ins_register_impl(ins_gps_passthrough_init);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
void ins_update_gps(void) {
if (gps.fix == GPS_FIX_3D) {
if (!ins_impl.ltp_initialized) {
ins_reset_local_origin();
}
/* simply scale and copy pos/speed from gps */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
INT32_VECT3_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
stateSetPositionNed_i(&ins_impl.ltp_pos);
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
INT32_VECT3_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
stateSetSpeedNed_i(&ins_impl.ltp_speed);
}
}
void follow_change_wp( unsigned char* buffer ) {
struct EcefCoor_i new_pos;
struct EnuCoor_i enu;
new_pos.x = DL_REMOTE_GPS_ecef_x(buffer);
new_pos.y = DL_REMOTE_GPS_ecef_y(buffer);
new_pos.z = DL_REMOTE_GPS_ecef_z(buffer);
// Translate to ENU
enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos);
INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
// TODO: Add the angle to the north
// Update the offsets
enu.x += POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
enu.y += POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
enu.z += POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
// TODO: Remove the angle to the north
// Move the waypoint
VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
}
