Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added GPS speed down to SITL state

Browse Source
This commit is contained in:
Andrew Tridgell 2013-03-28 10:28:44 +11:00
parent f6ddc4e4a1
commit 0ffc7dab6b
5 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -142,7 +142,7 @@ void SITL_State::_sitl_setup(void)
_update_barometer(_initial_height);
_update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
_update_compass(0, 0, 0);
_update_gps(0, 0, 0, 0, 0, false);
_update_gps(0, 0, 0, 0, 0, 0, false);
}
}
@ -231,7 +231,7 @@ void SITL_State::_timer_handler(int signum)
_simulator_output();
if (_update_count == 0 && _sitl != NULL) {
_update_gps(0, 0, 0, 0, 0, false);
_update_gps(0, 0, 0, 0, 0, 0, false);
_scheduler->timer_event();
_scheduler->sitl_end_atomic();
in_timer = false;
@ -249,7 +249,8 @@ void SITL_State::_timer_handler(int signum)
if (_sitl != NULL) {
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
_sitl->state.altitude,
_sitl->state.speedN, _sitl->state.speedE, !_sitl->gps_disable);
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
!_sitl->gps_disable);
_update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
_sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
_sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
@ -283,11 +284,11 @@ void SITL_State::_fdm_input(void)
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
switch (size) {
case 132:
case 140:
static uint32_t last_report;
static uint32_t count;
if (d.fg_pkt.magic != 0x4c56414e) {
if (d.fg_pkt.magic != 0x4c56414f) {
fprintf(stdout, "Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
return;
}

3
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -62,6 +62,7 @@ private:
float altitude;
double speedN;
double speedE;
double speedD;
bool have_lock;
};
@ -76,7 +77,7 @@ private:
static void _update_gps_mtk19(const struct gps_data *d);
static void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock);
double speedN, double speedE, double speedD, bool have_lock);
static void _update_ins(float roll, float pitch, float yaw, // Relative to earth
double rollRate, double pitchRate,double yawRate, // Local to plane

7
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -219,9 +219,9 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d)
velned.time = status.time;
velned.ned_north = 100.0 * d->speedN;
velned.ned_east = 100.0 * d->speedE;
velned.ned_down = 0;
velned.ned_down = 100.0 * d->speedD;
velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100;
velned.speed_3d = velned.speed_2d;
velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0;
@ -437,7 +437,7 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d)
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock)
double speedN, double speedE, double speedD, bool have_lock)
{
struct gps_data d;
char c;
@ -459,6 +459,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
d.altitude = altitude;
d.speedN = speedN;
d.speedE = speedE;
d.speedD = speedD;
d.have_lock = have_lock;
// add in some GPS lag

4
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -117,8 +117,8 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
q += _gyro_drift();
r += _gyro_drift();
_ins->set_gyro(Vector3f(p, q, r));
_ins->set_accel(Vector3f(xAccel, yAccel, zAccel));
_ins->set_gyro(Vector3f(p, q, r) + _ins->get_gyro_offsets());
_ins->set_accel(Vector3f(xAccel, yAccel, zAccel) + _ins->get_accel_offsets());
airspeed_pin_value = _airspeed_sensor(airspeed);
}

6
libraries/SITL/SITL.h
View File

@ -8,6 +8,7 @@
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#pragma pack(push,1)
struct sitl_fdm {
// this is the packet sent by the simulator
// to the APM executable to update the simulator state
@ -15,13 +16,14 @@ struct sitl_fdm {
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
double speedN, speedE; // m/s
double speedN, speedE, speedD; // m/s
double xAccel, yAccel, zAccel; // m/s/s in body frame
double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame
double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
double airspeed; // m/s
uint32_t magic; // 0x4c56414e
uint32_t magic; // 0x4c56414f
};
#pragma pack(pop)
class SITL