forked from Archive/PX4-Autopilot
support simulated airspeed
This commit is contained in:
parent
b2bef75bb4
commit
0fe272c9b3
|
@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len)
|
||||||
return _gps.copyData(buf, len);
|
return _gps.copyData(buf, len);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Simulator::getAirspeedSample(uint8_t *buf, int len)
|
||||||
|
{
|
||||||
|
return _airspeed.copyData(buf, len);
|
||||||
|
}
|
||||||
|
|
||||||
void Simulator::write_MPU_data(void *buf) {
|
void Simulator::write_MPU_data(void *buf) {
|
||||||
_mpu.writeData(buf);
|
_mpu.writeData(buf);
|
||||||
}
|
}
|
||||||
|
@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) {
|
||||||
_gps.writeData(buf);
|
_gps.writeData(buf);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Simulator::write_airspeed_data(void *buf) {
|
||||||
|
_airspeed.writeData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
int Simulator::start(int argc, char *argv[])
|
int Simulator::start(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
|
@ -94,6 +94,13 @@ struct RawBaroData {
|
||||||
};
|
};
|
||||||
#pragma pack(pop)
|
#pragma pack(pop)
|
||||||
|
|
||||||
|
#pragma pack(push, 1)
|
||||||
|
struct RawAirspeedData {
|
||||||
|
float temperature;
|
||||||
|
float diff_pressure;
|
||||||
|
};
|
||||||
|
#pragma pack(pop)
|
||||||
|
|
||||||
#pragma pack(push, 1)
|
#pragma pack(push, 1)
|
||||||
struct RawGPSData {
|
struct RawGPSData {
|
||||||
int32_t lat;
|
int32_t lat;
|
||||||
|
@ -191,12 +198,14 @@ public:
|
||||||
bool getMPUReport(uint8_t *buf, int len);
|
bool getMPUReport(uint8_t *buf, int len);
|
||||||
bool getBaroSample(uint8_t *buf, int len);
|
bool getBaroSample(uint8_t *buf, int len);
|
||||||
bool getGPSSample(uint8_t *buf, int len);
|
bool getGPSSample(uint8_t *buf, int len);
|
||||||
|
bool getAirspeedSample(uint8_t *buf, int len);
|
||||||
|
|
||||||
void write_MPU_data(void *buf);
|
void write_MPU_data(void *buf);
|
||||||
void write_accel_data(void *buf);
|
void write_accel_data(void *buf);
|
||||||
void write_mag_data(void *buf);
|
void write_mag_data(void *buf);
|
||||||
void write_baro_data(void *buf);
|
void write_baro_data(void *buf);
|
||||||
void write_gps_data(void *buf);
|
void write_gps_data(void *buf);
|
||||||
|
void write_airspeed_data(void *buf);
|
||||||
|
|
||||||
bool isInitialized() { return _initialized; }
|
bool isInitialized() { return _initialized; }
|
||||||
|
|
||||||
|
@ -207,6 +216,7 @@ private:
|
||||||
_baro(1),
|
_baro(1),
|
||||||
_mag(1),
|
_mag(1),
|
||||||
_gps(1),
|
_gps(1),
|
||||||
|
_airspeed(1),
|
||||||
_accel_pub(nullptr),
|
_accel_pub(nullptr),
|
||||||
_baro_pub(nullptr),
|
_baro_pub(nullptr),
|
||||||
_gyro_pub(nullptr),
|
_gyro_pub(nullptr),
|
||||||
|
@ -238,6 +248,7 @@ private:
|
||||||
simulator::Report<simulator::RawBaroData> _baro;
|
simulator::Report<simulator::RawBaroData> _baro;
|
||||||
simulator::Report<simulator::RawMagData> _mag;
|
simulator::Report<simulator::RawMagData> _mag;
|
||||||
simulator::Report<simulator::RawGPSData> _gps;
|
simulator::Report<simulator::RawGPSData> _gps;
|
||||||
|
simulator::Report<simulator::RawAirspeedData> _airspeed;
|
||||||
|
|
||||||
// uORB publisher handlers
|
// uORB publisher handlers
|
||||||
orb_advert_t _accel_pub;
|
orb_advert_t _accel_pub;
|
||||||
|
|
|
@ -187,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
|
||||||
|
|
||||||
RawBaroData baro;
|
RawBaroData baro;
|
||||||
// calculate air pressure from altitude (valid for low altitude)
|
// calculate air pressure from altitude (valid for low altitude)
|
||||||
baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt;
|
baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar
|
||||||
baro.altitude = imu->pressure_alt;
|
baro.altitude = imu->pressure_alt;
|
||||||
baro.temperature = imu->temperature;
|
baro.temperature = imu->temperature;
|
||||||
|
|
||||||
write_baro_data((void *)&baro);
|
write_baro_data((void *)&baro);
|
||||||
|
|
||||||
|
RawAirspeedData airspeed;
|
||||||
|
airspeed.temperature = imu->temperature;
|
||||||
|
airspeed.diff_pressure = imu->diff_pressure;
|
||||||
|
|
||||||
|
write_airspeed_data((void *)&airspeed);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
|
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
|
||||||
|
|
Loading…
Reference in New Issue