aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: added simulated MSP GPS

Browse Source
This commit is contained in:
Andrew Tridgell 2023-07-31 11:40:35 +10:00
parent 01c5f44556
commit 217f073165
3 changed files with 86 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/SITL/SIM_GPS.cpp
View File

@ -77,6 +77,7 @@ uint32_t GPS::device_baud() const
case Type::NMEA:
case Type::SBP:
case Type::SBP2:
case Type::MSP:
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
#endif
@ -1016,6 +1017,83 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc
return( crc );
}
/*
send MSP GPS data
*/
void GPS::update_msp(const struct gps_data *d)
{
struct PACKED {
// header
struct PACKED {
uint8_t dollar = '$';
uint8_t magic = 'X';
uint8_t code = '<';
uint8_t flags;
uint16_t cmd = 0x1F03; // GPS
uint16_t size = 52;
} hdr;
uint8_t instance;
uint16_t gps_week;
uint32_t ms_tow;
uint8_t fix_type;
uint8_t satellites_in_view;
uint16_t horizontal_pos_accuracy; // [cm]
uint16_t vertical_pos_accuracy; // [cm]
uint16_t horizontal_vel_accuracy; // [cm/s]
uint16_t hdop;
int32_t longitude;
int32_t latitude;
int32_t msl_altitude; // cm
int32_t ned_vel_north; // cm/s
int32_t ned_vel_east;
int32_t ned_vel_down;
uint16_t ground_course; // deg * 100, 0..36000
uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
// footer CRC
uint8_t crc;
} msp_gps {};
auto t = gps_time();
struct timeval tv;
simulation_timeval(&tv);
auto *tm = gmtime(&tv.tv_sec);
msp_gps.gps_week = t.week;
msp_gps.ms_tow = t.ms;
msp_gps.fix_type = d->have_lock?3:0;
msp_gps.satellites_in_view = d->have_lock?_sitl->gps_numsats[instance]:3;
msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.horizontal_vel_accuracy = 30;
msp_gps.hdop = 100;
msp_gps.longitude = d->longitude * 1.0e7;
msp_gps.latitude = d->latitude * 1.0e7;
msp_gps.msl_altitude = d->altitude * 100;
msp_gps.ned_vel_north = 100 * d->speedN;
msp_gps.ned_vel_east = 100 * d->speedE;
msp_gps.ned_vel_down = 100 * d->speedD;
msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100;
msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw
msp_gps.year = tm->tm_year;
msp_gps.month = tm->tm_mon;
msp_gps.day = tm->tm_mday;
msp_gps.hour = tm->tm_hour;
msp_gps.min = tm->tm_min;
msp_gps.sec = tm->tm_sec;
// CRC is over packet without first 3 bytes and trailing CRC byte
msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4);
write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps));
}
/*
read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
*/
@ -1218,6 +1296,10 @@ void GPS::update()
update_nova(&d);
break;
case Type::MSP:
update_msp(&d);
break;
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
update_file();
@ -1254,7 +1336,7 @@ GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms)
d2.speedN += p * (s2.speedN - s1.speedN);
d2.speedE += p * (s2.speedE - s1.speedE);
d2.speedD += p * (s2.speedD - s1.speedD);
d2.yaw_deg += p * (s2.yaw_deg - s1.yaw_deg);
d2.yaw_deg += p * wrap_180(s2.yaw_deg - s1.yaw_deg);
return d2;
}
}

2
libraries/SITL/SIM_GPS.h
View File

@ -59,6 +59,7 @@ public:
#endif
NOVA = 8,
SBP2 = 9,
MSP = 19,
};
GPS(uint8_t _instance);
@ -115,6 +116,7 @@ private:
void update_sbp(const struct gps_data *d);
void update_sbp2(const struct gps_data *d);
void update_msp(const struct gps_data *d);
#if AP_SIM_GPS_FILE_ENABLED
void update_file();

2
libraries/SITL/SITL.cpp
View File

@ -437,7 +437,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
// @Param: GPS_TYPE
// @DisplayName: GPS 1 type
// @Description: Sets the type of simulation used for GPS 1
// @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:GSOF
// @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:GSOF, 19:MSP
// @User: Advanced
AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 4, SIM, gps_byteloss[0], 0),