Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: Send NMEA GPHDT sentence if enabled. 

and add VTG message
This commit is contained in:
Grant Morphett 2017-05-28 14:09:58 +10:00 committed by Andrew Tridgell
parent 5c3afa2621
commit fdcdbddecc
3 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -85,7 +85,7 @@ void SITL_State::_sitl_setup(const char *home_str)
// setup some initial values
#ifndef HIL_MODE
_update_airspeed(0);
_update_gps(0, 0, 0, 0, 0, 0, false);
_update_gps(0, 0, 0, 0, 0, 0, 0, false);
_update_rangefinder(0);
#endif
if (enable_gimbal) {
@ -170,7 +170,7 @@ void SITL_State::_fdm_input_step(void)
_scheduler->sitl_begin_atomic();
if (_update_count == 0 && _sitl != nullptr) {
_update_gps(0, 0, 0, 0, 0, 0, false);
_update_gps(0, 0, 0, 0, 0, 0, 0, false);
_scheduler->timer_event();
_scheduler->sitl_end_atomic();
return;
@ -180,6 +180,7 @@ void SITL_State::_fdm_input_step(void)
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
_sitl->state.altitude,
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
_sitl->state.yawDeg,
!_sitl->gps_disable);
_update_airspeed(_sitl->state.airspeed);
_update_rangefinder(_sitl->state.range);

4
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -102,6 +102,7 @@ private:
double speedN;
double speedE;
double speedD;
double yaw;
bool have_lock;
};
@ -129,7 +130,8 @@ private:
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock);
double speedN, double speedE, double speedD,
double yaw, bool have_lock);
void _update_airspeed(float airspeed);
void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
void _check_rc_input(void);

19
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -675,10 +675,20 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
2.0,
d->altitude);
float speed_knots = norm(d->speedN, d->speedE) * M_PER_SEC_TO_KNOTS;
float heading = ToDeg(atan2f(d->speedE, d->speedN));
if (heading < 0) {
heading += 360.0f;
}
//$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24
_gps_nmea_printf(instance, "$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
tstring,
heading,
heading,
speed_knots,
speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
_gps_nmea_printf(instance, "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
tstring,
d->have_lock?'A':'V',
@ -687,6 +697,10 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
speed_knots,
heading,
dstring);
if (_sitl->gps_hdg_enabled) {
_gps_nmea_printf(instance, "$GPHDT,%.2f,T", d->yaw);
}
}
void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance)
@ -1151,7 +1165,8 @@ void SITL_State::_update_gps_file(uint8_t instance)
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock)
double speedN, double speedE, double speedD,
double yaw, bool have_lock)
{
struct gps_data d;
char c;
@ -1195,6 +1210,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
d.latitude = latitude;
d.longitude = longitude;
d.yaw = yaw;
// add an altitude error controlled by a slow sine wave
d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);