Skip to content

Q
qgroundcontrol
Commit d18ea79a authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge branch 'sensor_hil' of github.com:mavlink/qgroundcontrol into sensor_hil

parents b6076325 965cd4e6
Show whitespace changes
Inline Side-by-side
Showing with 64 additions and 17 deletions
src/comm/QGCXPlaneLink.cc
View file @ d18ea79a
......@@ -354,6 +354,35 @@ void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitc
writeBytes((const char*)&p, sizeof(p));
}
Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
double c__ = cos(yaw);
double _c_ = cos(pitch);
double __c = cos(roll);
double s__ = sin(yaw);
double _s_ = sin(pitch);
double __s = sin(roll);
double cc_ = c__ * _c_;
double cs_ = c__ * _s_;
double sc_ = s__ * _c_;
double ss_ = s__ * _s_;
double c_c = c__ * __c;
double c_s = c__ * __s;
double s_c = s__ * __c;
double s_s = s__ * __s;
double _cc = _c_ * __c;
double _cs = _c_ * __s;
double csc = cs_ * __c;
double css = cs_ * __s;
double ssc = ss_ * __c;
double sss = ss_ * __s;
Eigen::Matrix3f wRo;
wRo <<
cc_ , css-s_c, csc+s_s,
sc_ , sss+c_c, ssc-c_s,
-_s_ , _cs, _cc;
return wRo;
}
void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
{
if (!data) return;
......@@ -442,6 +471,12 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
}
if (p.index == 4)
{
xacc = p.f[5] * 9.81f;
xacc = p.f[6] * 9.81f;
zacc = -p.f[4] * 9.81f;
}
else if (p.index == 6 && xPlaneVersion == 10)
{
// inHg to kPa
......@@ -518,8 +553,8 @@ void QGCXPlaneLink::readBytes()
else if (p.index == 21 && xPlaneVersion == 10)
{
vx = p.f[3];
vy = p.f[4];
vz = p.f[5];
vy = -p.f[5];
vz = p.f[4];
}
else if (p.index == 12)
{
......@@ -593,8 +628,8 @@ void QGCXPlaneLink::readBytes()
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
} else {
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, xacc, yacc, zacc);
}
}
......@@ -643,8 +678,8 @@ bool QGCXPlaneLink::disconnectSimulation()
disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
disconnect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int)));
disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
UAS* uas = dynamic_cast<UAS*>(mav);
......@@ -827,8 +862,8 @@ bool QGCXPlaneLink::connectSimulation()
connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
connect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int)));
connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
UAS* uas = dynamic_cast<UAS*>(mav);
......
src/uas/UAS.cc
View file @ d18ea79a
......@@ -1691,10 +1691,22 @@ QList<int> UAS::getComponentIds()
void UAS::setMode(int mode)
{
//this->mode = mode; //no call assignament, update receive message from UAS
// Strip armed / disarmed call, this is not relevant for setting the mode
uint8_t newMode = mode;
newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED);
// Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED);
// Strip HIL part, replace it with current system state
newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED);
// Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED);
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode);
sendMessage(msg);
qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newMode;
}
/**
......@@ -2720,7 +2732,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
mavlink_message_t msg;
mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed,
lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000);
sendMessage(msg);
}
else
......
src/ui/uas/UASControlWidget.cc
View file @ d18ea79a
......@@ -51,12 +51,12 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
ui.modeComboBox->clear();
ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT), MAV_MODE_PREFLIGHT);
ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT).remove(0, 2), MAV_MODE_PREFLIGHT);
ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED).remove(0, 2), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment