diff --git a/conf/modules/gps_datalink.xml b/conf/modules/gps_datalink.xml index c139793660..eebd72b4fa 100644 --- a/conf/modules/gps_datalink.xml +++ b/conf/modules/gps_datalink.xml @@ -17,6 +17,7 @@ + diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 9d0d990ab5..6d372c3b82 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -173,6 +173,60 @@ static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); } +/** Parse the REMOTE_GPS_LOCAL datalink packet */ +static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z, + float enu_xd, float enu_yd, float enu_zd, + uint32_t tow, float course) +{ + + struct EnuCoor_i enu_pos, enu_speed; + + // Position in ENU coordinates + enu_pos.x = (int32_t)CM_OF_M(enu_x); + enu_pos.y = (int32_t)CM_OF_M(enu_y); + enu_pos.z = (int32_t)CM_OF_M(enu_z); + + // Convert the ENU coordinates to ECEF + ecef_of_enu_point_i(&gps_datalink.ecef_pos, <p_def, &enu_pos); + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT); + + lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos); + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT); + + enu_speed.x = (int32_t)CM_OF_M(enu_xd); + enu_speed.y = (int32_t)CM_OF_M(enu_yd); + enu_speed.z = (int32_t)CM_OF_M(enu_zd); + + VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed); + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT); + + ecef_of_enu_vect_i(&gps_datalink.ecef_vel , <p_def , &enu_speed); + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT); + + gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed); + gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed); + + gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10; + SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT); + + gps_datalink.course = (int32_t)(RadOfDeg(course)*1e7); + SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT); + + gps_datalink.num_sv = 7; + gps_datalink.tow = tow; + gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true + + // set gps msg time + gps_datalink.last_msg_ticks = sys_time.nb_sec_rem; + gps_datalink.last_msg_time = sys_time.nb_sec; + + gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_datalink.last_3dfix_time = sys_time.nb_sec; + + // publish new GPS data + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); +} void gps_datalink_parse_REMOTE_GPS(void) { @@ -202,3 +256,17 @@ void gps_datalink_parse_REMOTE_GPS_SMALL(void) DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer), DL_REMOTE_GPS_SMALL_tow(dl_buffer)); } + +void gps_datalink_parse_REMOTE_GPS_LOCAL(void) +{ + if (DL_REMOTE_GPS_LOCAL_ac_id(dl_buffer) != AC_ID) { return; } // not for this aircraft + + parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(dl_buffer), + DL_REMOTE_GPS_LOCAL_enu_y(dl_buffer), + DL_REMOTE_GPS_LOCAL_enu_z(dl_buffer), + DL_REMOTE_GPS_LOCAL_enu_xd(dl_buffer), + DL_REMOTE_GPS_LOCAL_enu_yd(dl_buffer), + DL_REMOTE_GPS_LOCAL_enu_zd(dl_buffer), + DL_REMOTE_GPS_LOCAL_tow(dl_buffer), + DL_REMOTE_GPS_LOCAL_course(dl_buffer)); +} diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index bf8e6dc251..2a83829bc1 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -46,5 +46,6 @@ extern void gps_datalink_init(void); extern void gps_datalink_parse_REMOTE_GPS(void); extern void gps_datalink_parse_REMOTE_GPS_SMALL(void); +extern void gps_datalink_parse_REMOTE_GPS_LOCAL(void); #endif /* GPS_DATALINK_H */ diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 9525de7663..7dcd0843a1 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 9525de76635aac9b6dc058cc3940fa8fff17cd53 +Subproject commit 7dcd0843a1a935ab80f20a0a1b91659698d1e4b7