Commit 09d4e8ec authored by LM's avatar LM

Fixed a few v10 porting issues with PIXHAWK messages, improved GPS display

parent ceb4216c
#include "LinkInterface.h"
//#include "LinkInterface.h"
LinkInterface::~LinkInterface()
{
emit this->deleteLink(this);
}
\ No newline at end of file
//LinkInterface::~LinkInterface()
//{
//}
......@@ -44,7 +44,7 @@ class LinkInterface : public QThread
Q_OBJECT
public:
LinkInterface(QObject* parent = 0) : QThread(parent) {}
virtual ~LinkInterface() {}
virtual ~LinkInterface() { emit this->deleteLink(this); }
/* Connection management */
......
......@@ -560,20 +560,6 @@ void MAVLinkSimulationLink::mainloop()
// Send controller states
#ifdef MAVLINK_ENABLED_PIXHAWK
uint8_t attControl = 1;
uint8_t posXYControl = 1;
uint8_t posZControl = 0;
uint8_t posYawControl = 1;
uint8_t gpsLock = 2;
uint8_t visLock = 3;
uint8_t ahrsHealth = 200;
uint8_t posLock = qMax(gpsLock, visLock);
messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, ahrsHealth, attControl, posXYControl, posZControl, posYawControl);
#endif
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
memcpy(stream+streampointer, buffer, bufferlength);
streampointer += bufferlength;
......
......@@ -206,21 +206,6 @@ void MAVLinkSimulationMAV::mainloop()
// The message container to be used for sending
mavlink_message_t ret;
#ifdef MAVLINK_ENABLED_PIXHAWK
// Send which controllers are active
mavlink_control_status_t control_status;
control_status.control_att = 1;
control_status.control_pos_xy = 1;
control_status.control_pos_yaw = 1;
control_status.control_pos_z = 1;
control_status.gps_fix = 2; // 2D GPS fix
control_status.position_fix = 3; // 3D fix from GPS + barometric pressure
control_status.vision_fix = 0; // no fix from vision system
control_status.ahrs_health = 230;
mavlink_msg_control_status_encode(systemid, MAV_COMP_ID_IMU, &ret, &control_status);
link->sendMAVLinkMessage(&ret);
#endif //MAVLINK_ENABLED_PIXHAWK
// Send actual controller outputs
// This message just shows the direction
// and magnitude of the control output
......
......@@ -346,7 +346,7 @@ QString aircraft("Rascal110-JSBSim");
#ifdef Q_OS_MACX
processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs";
fgRoot = "--fg-root=/Applications/FlightGear.app/Contents/Resources/data";
fgScenery = "--fg-scenery=\"/Applications/FlightGear.app/Contents/Resources/data/Scenery\"";
fgScenery = "--fg-scenery=/Applications/FlightGear.app/Contents/Resources/data/Scenery";
#endif
#ifdef Q_OS_WIN32
......@@ -359,8 +359,8 @@ processFgfs = "fgfs";
fgRoot = "--fg-root=/usr/share/flightgear/data";
#endif
//processCall << fgRoot;
//processCall << fgScenery;
processCall << fgRoot;
processCall << fgScenery;
processCall << "--generic=socket,out,50,127.0.0.1,49005,udp,ardupilot";
processCall << "--generic=socket,in,50,127.0.0.1,49000,udp,ardupilot";
processCall << "--in-air";
......
......@@ -123,18 +123,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
}
break;
case MAVLINK_MSG_ID_AUX_STATUS: {
mavlink_aux_status_t status;
mavlink_msg_aux_status_decode(&message, &status);
emit loadChanged(this, status.load/10.0f);
emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
emit valueChanged(uasId, "Load", "%", ((float)status.load)/10.0f, getUnixTime());
}
break;
default:
// Let UAS handle the default message set
UAS::receiveMessage(link, message);
......
......@@ -294,7 +294,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
else if (modechanged || statechanged)
{
GAudioOutput::instance()->stopEmergency();
GAudioOutput::instance()->say(audiostring);
GAudioOutput::instance()->say(audiostring.toLower());
}
if (state.system_status == MAV_STATE_POWEROFF)
......@@ -345,24 +345,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
break;
#ifdef MAVLINK_ENABLED_PIXHAWK
case MAVLINK_MSG_ID_CONTROL_STATUS:
{
mavlink_control_status_t status;
mavlink_msg_control_status_decode(&message, &status);
// Emit control status vector
emit attitudeControlEnabled(static_cast<bool>(status.control_att));
emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));
// Emit localization status vector
emit localizationChanged(this, status.position_fix);
emit visionLocalizationChanged(this, status.vision_fix);
emit gpsLocalizationChanged(this, status.gps_fix);
}
break;
#endif // PIXHAWK
case MAVLINK_MSG_ID_RAW_IMU:
{
mavlink_raw_imu_t raw;
......@@ -625,14 +607,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// quint64 time = getUnixTime(pos.usec);
quint64 time = getUnixTime(pos.usec);
emit valueChanged(uasId, "latitude", "deg", pos.lat/(double)1E7, time);
emit valueChanged(uasId, "longitude", "deg", pos.lon/(double)1E7, time);
emit valueChanged(uasId, "eph", "m", pos.eph/(double)1E2, time);
emit valueChanged(uasId, "epv", "m", pos.eph/(double)1E2, time);
emit valueChanged(uasId, "gps latitude", "deg", pos.lat/(double)1E7, time);
emit valueChanged(uasId, "gps longitude", "deg", pos.lon/(double)1E7, time);
emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time);
emit valueChanged(uasId, "gps eph", "m", pos.eph/(double)1E2, time);
emit valueChanged(uasId, "gps epv", "m", pos.eph/(double)1E2, time);
emit valueChanged(uasId, "gps fix", "raw", pos.fix_type, time);
emit valueChanged(uasId, "gps heading", "raw", pos.hdg, time);
if (pos.fix_type > 2) {
emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time);
latitude = pos.lat/(double)1E7;
longitude = pos.lon/(double)1E7;
altitude = pos.alt/1000.0;
......@@ -2550,7 +2534,7 @@ void UAS::startLowBattAlarm()
{
if (!lowBattAlarm)
{
GAudioOutput::instance()->alert(tr("SYSTEM %1 HAS LOW BATTERY").arg(getUASName()));
GAudioOutput::instance()->alert(tr("system %1 has low battery").arg(getUASName()));
QTimer::singleShot(2500, GAudioOutput::instance(), SLOT(startEmergency()));
lowBattAlarm = true;
}
......
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