mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_SITL: Send NMEA GPHDT sentence if enabled.
and add VTG message
This commit is contained in:
parent
5c3afa2621
commit
fdcdbddecc
|
@ -85,7 +85,7 @@ void SITL_State::_sitl_setup(const char *home_str)
|
||||||
// setup some initial values
|
// setup some initial values
|
||||||
#ifndef HIL_MODE
|
#ifndef HIL_MODE
|
||||||
_update_airspeed(0);
|
_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);
|
_update_rangefinder(0);
|
||||||
#endif
|
#endif
|
||||||
if (enable_gimbal) {
|
if (enable_gimbal) {
|
||||||
|
@ -170,7 +170,7 @@ void SITL_State::_fdm_input_step(void)
|
||||||
_scheduler->sitl_begin_atomic();
|
_scheduler->sitl_begin_atomic();
|
||||||
|
|
||||||
if (_update_count == 0 && _sitl != nullptr) {
|
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->timer_event();
|
||||||
_scheduler->sitl_end_atomic();
|
_scheduler->sitl_end_atomic();
|
||||||
return;
|
return;
|
||||||
|
@ -180,6 +180,7 @@ void SITL_State::_fdm_input_step(void)
|
||||||
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
|
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
|
||||||
_sitl->state.altitude,
|
_sitl->state.altitude,
|
||||||
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
|
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
|
||||||
|
_sitl->state.yawDeg,
|
||||||
!_sitl->gps_disable);
|
!_sitl->gps_disable);
|
||||||
_update_airspeed(_sitl->state.airspeed);
|
_update_airspeed(_sitl->state.airspeed);
|
||||||
_update_rangefinder(_sitl->state.range);
|
_update_rangefinder(_sitl->state.range);
|
||||||
|
|
|
@ -102,6 +102,7 @@ private:
|
||||||
double speedN;
|
double speedN;
|
||||||
double speedE;
|
double speedE;
|
||||||
double speedD;
|
double speedD;
|
||||||
|
double yaw;
|
||||||
bool have_lock;
|
bool have_lock;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -129,7 +130,8 @@ private:
|
||||||
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
|
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
|
||||||
|
|
||||||
void _update_gps(double latitude, double longitude, float altitude,
|
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_airspeed(float airspeed);
|
||||||
void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
|
void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
|
||||||
void _check_rc_input(void);
|
void _check_rc_input(void);
|
||||||
|
|
|
@ -675,10 +675,20 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
|
||||||
2.0,
|
2.0,
|
||||||
d->altitude);
|
d->altitude);
|
||||||
float speed_knots = norm(d->speedN, d->speedE) * M_PER_SEC_TO_KNOTS;
|
float speed_knots = norm(d->speedN, d->speedE) * M_PER_SEC_TO_KNOTS;
|
||||||
|
|
||||||
float heading = ToDeg(atan2f(d->speedE, d->speedN));
|
float heading = ToDeg(atan2f(d->speedE, d->speedN));
|
||||||
if (heading < 0) {
|
if (heading < 0) {
|
||||||
heading += 360.0f;
|
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,,",
|
_gps_nmea_printf(instance, "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
|
||||||
tstring,
|
tstring,
|
||||||
d->have_lock?'A':'V',
|
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,
|
speed_knots,
|
||||||
heading,
|
heading,
|
||||||
dstring);
|
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)
|
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
|
possibly send a new GPS packet
|
||||||
*/
|
*/
|
||||||
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
|
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;
|
struct gps_data d;
|
||||||
char c;
|
char c;
|
||||||
|
@ -1195,6 +1210,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
|
||||||
|
|
||||||
d.latitude = latitude;
|
d.latitude = latitude;
|
||||||
d.longitude = longitude;
|
d.longitude = longitude;
|
||||||
|
d.yaw = yaw;
|
||||||
|
|
||||||
// add an altitude error controlled by a slow sine wave
|
// add an altitude error controlled by a slow sine wave
|
||||||
d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);
|
d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue