Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[gps_datalink] update small message protocol

Browse Source
This commit is contained in:
kirkscheper
2016-01-06 17:45:53 +01:00
parent 47409e0b88
commit 449ec64297
9 changed files with 299 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
-9
View File
@@ -161,13 +161,6 @@
<define name="USE_GPS_ALT" value="1"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
@@ -183,8 +176,6 @@
<define name="H_Z" value="0.92060036"/>
<define name="USE_GPS_HEADING" value="1"/>
</section>
<define name="USE_GPS" value="TRUE"/>
<define name="USE_GPS_HEADING" value="TRUE"/>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="290"/>
conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml
-7
View File
@@ -173,13 +173,6 @@
<define name="USE_GPS_ALT" value="1"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml
-7
View File
@@ -102,13 +102,6 @@
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="140" unit="deg/s"/>
conf/messages.xml
+3 -3
View File
@@ -856,7 +856,6 @@
<message name="BLUEGIGA" id="106">
<field name="data_rate" type="uint32" unit="bytes/s"/>
<field name="last_msg" type="uint8[]"/>
</message>
<!--107 is free -->
@@ -2479,8 +2478,9 @@
<message name="REMOTE_GPS_SMALL" id="54" link="broadcasted">
<field name="ac_id" type="uint8"/>
<field name="numsv" type="uint8"/>
<field name="pos_xyz" type="uint32" unit="cm">bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free</field>
<field name="speed_xy" type="uint32" unit="cm/s">bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free</field>
<field name="pos_xyz" type="uint32" unit="cm">bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm</field>
<field name="speed_xyz" type="uint32" unit="cm/s">bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s</field>
<field name="heading" type="int16" unit="rad*1e4"/>
</message>
<message name="REMOTE_GPS" id="55" link="broadcasted">
sw/airborne/firmwares/rotorcraft/datalink.c
+20 -18
View File
@@ -90,7 +90,7 @@ void dl_parse_msg(void)
}
break;
#if defined USE_NAVIGATION
#ifdef USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
@@ -138,21 +138,21 @@ void dl_parse_msg(void)
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if defined GPS_DATALINK
#ifdef GPS_USE_DATALINK_SMALL
case DL_REMOTE_GPS_SMALL :
#if USE_GPS
#ifdef GPS_DATALINK
case DL_REMOTE_GPS_SMALL : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
break;
}
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
parse_gps_datalink_small(
DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
break;
#endif
case DL_REMOTE_GPS :
DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_heading(dl_buffer));
}
break;
case DL_REMOTE_GPS : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
@@ -171,10 +171,11 @@ void dl_parse_msg(void)
DL_REMOTE_GPS_ecef_zd(dl_buffer),
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
break;
#endif
#if USE_GPS
case DL_GPS_INJECT :
}
break;
#endif // GPS_DATALINK
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
@@ -183,9 +184,10 @@ void dl_parse_msg(void)
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
break;
#endif
);
}
break;
#endif // USE_GPS
case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
sw/airborne/subsystems/gps/gps_datalink.c
+57 -66
View File
@@ -27,35 +27,19 @@
* GPS structure to the values received.
*/
#include "messages.h"
#include "generated/airframe.h" // AC_ID
#include "generated/flight_plan.h" // reference lla NAV_XXX0
#include "subsystems/datalink/downlink.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
// #include <stdio.h>
#if GPS_USE_DATALINK_SMALL
#ifndef GPS_LOCAL_ECEF_ORIGIN_X
#error Local x coordinate in ECEF of the remote GPS required
#endif
#ifndef GPS_LOCAL_ECEF_ORIGIN_Y
#error Local y coordinate in ECEF of the remote GPS required
#endif
#ifndef GPS_LOCAL_ECEF_ORIGIN_Z
#error Local z coordinate in ECEF of the remote GPS required
#endif
struct EcefCoor_i tracking_ecef;
struct LtpDef_i tracking_ltp;
// #include <stdio.h>
struct LtpDef_i ltp_def;
struct EnuCoor_i enu_pos, enu_speed;
struct EcefCoor_i ecef_pos, ecef_vel;
struct LlaCoor_i lla_pos;
#endif
bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed
/** GPS initialization */
@@ -66,70 +50,68 @@ void gps_impl_init(void)
gps.gspeed = 700; // To enable course setting
gps.cacc = 0; // To enable course setting
#if GPS_USE_DATALINK_SMALL
tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X;
tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y;
tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z;
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef);
#endif
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
}
#ifdef GPS_USE_DATALINK_SMALL
// Parse the REMOTE_GPS_SMALL datalink packet
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy)
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
{
// Position in ENU coordinates
enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm
if (enu_pos.x & 0x200) {
enu_pos.x |= 0xFFFFFC00; // fix for twos complements
enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
if (enu_pos.x & 0x400) {
enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
}
enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm
if (enu_pos.y & 0x200) {
enu_pos.y |= 0xFFFFFC00; // fix for twos complements
enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
if (enu_pos.y & 0x400) {
enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
}
enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm
// bits 1 and 0 are free
// printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z);
enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
// Convert the ENU coordinates to ECEF
ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos);
gps.ecef_pos = ecef_pos;
ecef_of_enu_point_i(&gps.ecef_pos, &ltp_def, &enu_pos);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
lla_of_ecef_i(&lla_pos, &ecef_pos);
gps.lla_pos = lla_pos;
lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s
if (enu_speed.x & 0x200) {
enu_speed.x |= 0xFFFFFC00; // fix for twos complements
enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
if (enu_speed.x & 0x400) {
enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
}
enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s
if (enu_speed.y & 0x200) {
enu_speed.y |= 0xFFFFFC00; // fix for twos complements
enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
if (enu_speed.y & 0x400) {
enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
}
enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
if (enu_speed.z & 0x200) {
enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
}
enu_speed.z = 0;
// printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z);
ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed);
ecef_of_enu_vect_i(&gps.ecef_vel , &ltp_def , &enu_speed);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy
// todo check correct
gps.ned_vel.x = enu_speed.x;
gps.ned_vel.y = enu_speed.y;
gps.ned_vel.z = -enu_speed.z;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
gps.hmsl = ltp_def.hmsl + enu_pos.z * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2
if (gps.course & 0x200) {
gps.course |= 0xFFFFFC00; // fix for twos complements
}
// printf("Heading: %d\n", gps.course);
gps.course *= 1e5;
gps.course = ((int32_t)heading) * 1e3;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = num_sv;
gps.tow = 0; // set time-of-weak to 0
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // todo check
gps.fix = GPS_FIX_3D; // set 3D fix to true
gps_available = TRUE; // set GPS available to true
@@ -143,7 +125,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
}
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
}
#endif
/** Parse the REMOTE_GPS datalink packet */
void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
@@ -168,10 +149,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
gps.ecef_vel.z = ecef_zd;
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
struct LtpDef_i ref_ltp;
ltp_def_from_ecef_i(&ref_ltp, &gps.ecef_pos);
ned_of_ecef_vect_i(&gps.ned_vel, &ref_ltp, &gps.ecef_vel);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
gps.course = course;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = numsv;
gps.tow = tow;
if (tow == 0) {
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
} else {
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
}
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
sw/airborne/subsystems/gps/gps_datalink.h
+3 -3
View File
@@ -35,9 +35,9 @@
#define GPS_NB_CHANNELS 0
extern bool_t gps_available;
#ifdef GPS_USE_DATALINK_SMALL
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy);
#endif
extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading);
extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
sw/airborne/subsystems/ins/ins_int.c
+5
View File
@@ -98,6 +98,10 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
#define INS_VFF_R_GPS 2.0
#endif
#ifndef INS_VFF_VZ_R_GPS
#define INS_VFF_VZ_R_GPS 2.0
#endif
/** maximum number of propagation steps without any updates in between */
#ifndef INS_MAX_PROPAGATION_STEPS
#define INS_MAX_PROPAGATION_STEPS 200
@@ -374,6 +378,7 @@ void ins_int_update_gps(struct GpsState *gps_s)
#if INS_USE_GPS_ALT
vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
#endif
#if USE_HFF
sw/ground_segment/misc/natnet2ivy.c
+211 -166
View File
File diff suppressed because it is too large Load Diff