@ -201,16 +201,19 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -201,16 +201,19 @@ void UAS::receiveMessage(mavlink_message_t message)
componentName = " ANONYMOUS " ;
break ;
}
case MAV_COMP_ID_IMU :
{
componentName = " IMU #1 " ;
break ;
}
case MAV_COMP_ID_CAMERA :
{
componentName = " CAMERA " ;
break ;
}
case MAV_COMP_ID_MISSIONPLANNER :
{
componentName = " MISSIONPLANNER " ;
@ -240,6 +243,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -240,6 +243,7 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer IMU 2 over IMU 1 (FIXME)
componentID [ message . msgid ] = MAV_COMP_ID_IMU_2 ;
break ;
default :
// Do nothing
break ;
@ -251,6 +255,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -251,6 +255,7 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer the first component
componentID [ message . msgid ] = message . compid ;
}
else
{
// Got this message already
@ -272,6 +277,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -272,6 +277,7 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break ;
}
mavlink_heartbeat_t state ;
mavlink_msg_heartbeat_decode ( & message , & state ) ;
@ -303,6 +309,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -303,6 +309,7 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break ;
}
mavlink_sys_status_t state ;
mavlink_msg_sys_status_decode ( & message , & state ) ;
@ -336,10 +343,12 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -336,10 +343,12 @@ void UAS::receiveMessage(mavlink_message_t message)
{
state . drop_rate_comm = 10000 ;
}
emit dropRateChanged ( this - > getUASID ( ) , state . drop_rate_comm / 100.0f ) ;
emit valueChanged ( uasId , name . arg ( " drop_rate_comm " ) , " % " , state . drop_rate_comm / 100.0f , time ) ;
}
break ;
case MAVLINK_MSG_ID_ATTITUDE :
{
mavlink_attitude_t attitude ;
@ -361,6 +370,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -361,6 +370,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION :
{
mavlink_attitude_quaternion_t attitude ;
@ -390,17 +400,24 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -390,17 +400,24 @@ void UAS::receiveMessage(mavlink_message_t message)
float phi , theta , psi ;
theta = asin ( - dcm [ 2 ] [ 0 ] ) ;
if ( fabs ( theta - M_PI_2 ) < 1.0e-3 f ) {
if ( fabs ( theta - M_PI_2 ) < 1.0e-3 f )
{
phi = 0.0f ;
psi = ( atan2 ( dcm [ 1 ] [ 2 ] - dcm [ 0 ] [ 1 ] ,
dcm [ 0 ] [ 2 ] + dcm [ 1 ] [ 1 ] ) + phi ) ;
} else if ( fabs ( theta + M_PI_2 ) < 1.0e-3 f ) {
}
else if ( fabs ( theta + M_PI_2 ) < 1.0e-3 f )
{
phi = 0.0f ;
psi = atan2f ( dcm [ 1 ] [ 2 ] - dcm [ 0 ] [ 1 ] ,
dcm [ 0 ] [ 2 ] + dcm [ 1 ] [ 1 ] - phi ) ;
} else {
}
else
{
phi = atan2f ( dcm [ 2 ] [ 1 ] , dcm [ 2 ] [ 2 ] ) ;
psi = atan2f ( dcm [ 1 ] [ 0 ] , dcm [ 0 ] [ 0 ] ) ;
}
@ -422,6 +439,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -422,6 +439,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_HIL_CONTROLS :
{
mavlink_hil_controls_t hil ;
@ -429,6 +447,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -429,6 +447,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit hilControlsChanged ( hil . time_usec , hil . roll_ailerons , hil . pitch_elevator , hil . yaw_rudder , hil . throttle , hil . mode , hil . nav_mode ) ;
}
break ;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS :
{
mavlink_hil_actuator_controls_t hil ;
@ -453,6 +472,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -453,6 +472,7 @@ void UAS::receiveMessage(mavlink_message_t message)
hil . mode ) ;
}
break ;
case MAVLINK_MSG_ID_VFR_HUD :
{
mavlink_vfr_hud_t hud ;
@ -469,13 +489,16 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -469,13 +489,16 @@ void UAS::receiveMessage(mavlink_message_t message)
setAltitudeAMSL ( hud . alt ) ;
setGroundSpeed ( hud . groundspeed ) ;
if ( ! qIsNaN ( hud . airspeed ) )
setAirSpeed ( hud . airspeed ) ;
speedZ = - hud . climb ;
emit altitudeChanged ( this , altitudeAMSL , altitudeRelative , - speedZ , time ) ;
emit speedChanged ( this , groundSpeed , airSpeed , time ) ;
}
break ;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED :
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@ -495,6 +518,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -495,6 +518,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE :
{
mavlink_global_vision_position_estimate_t pos ;
@ -503,6 +527,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -503,6 +527,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit attitudeChanged ( this , message . compid , pos . roll , pos . pitch , pos . yaw , time ) ;
}
break ;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT :
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@ -533,6 +558,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -533,6 +558,7 @@ void UAS::receiveMessage(mavlink_message_t message)
isGlobalPositionKnown = true ;
}
break ;
case MAVLINK_MSG_ID_GPS_RAW_INT :
{
mavlink_gps_raw_int_t pos ;
@ -542,10 +568,12 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -542,10 +568,12 @@ void UAS::receiveMessage(mavlink_message_t message)
// TODO: track localization state not only for gps but also for other loc. sources
int loc_type = pos . fix_type ;
if ( loc_type = = 1 )
{
loc_type = 0 ;
}
setSatelliteCount ( pos . satellites_visible ) ;
if ( pos . fix_type > 2 )
@ -557,18 +585,24 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -557,18 +585,24 @@ void UAS::receiveMessage(mavlink_message_t message)
altitude_gps = pos . alt / 1000.0 ;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if ( ! globalEstimatorActive ) {
if ( ! globalEstimatorActive )
{
setLatitude ( latitude_gps ) ;
setLongitude ( longitude_gps ) ;
emit globalPositionChanged ( this , getLatitude ( ) , getLongitude ( ) , getAltitudeAMSL ( ) , time ) ;
emit altitudeChanged ( this , altitudeAMSL , altitudeRelative , - speedZ , time ) ;
float vel = pos . vel / 100.0f ;
// Smaller than threshold and not NaN
if ( ( vel < 1000000 ) & & ! qIsNaN ( vel ) & & ! qIsInf ( vel ) ) {
if ( ( vel < 1000000 ) & & ! qIsNaN ( vel ) & & ! qIsInf ( vel ) )
{
setGroundSpeed ( vel ) ;
emit speedChanged ( this , groundSpeed , airSpeed , time ) ;
} else {
}
else
{
emit textMessageReceived ( uasId , message . compid , MAV_SEVERITY_NOTICE , QString ( " GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s " ) . arg ( vel ) ) ;
}
}
@ -577,18 +611,23 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -577,18 +611,23 @@ void UAS::receiveMessage(mavlink_message_t message)
double dtmp ;
//-- Raw GPS data
dtmp = pos . eph = = 0xFFFF ? 1e10 f : pos . eph / 100.0 ;
if ( dtmp ! = satRawHDOP )
{
satRawHDOP = dtmp ;
emit satRawHDOPChanged ( satRawHDOP ) ;
}
dtmp = pos . epv = = 0xFFFF ? 1e10 f : pos . epv / 100.0 ;
if ( dtmp ! = satRawVDOP )
{
satRawVDOP = dtmp ;
emit satRawVDOPChanged ( satRawVDOP ) ;
}
dtmp = pos . cog = = 0xFFFF ? 0.0 : pos . cog / 100.0 ;
if ( dtmp ! = satRawCOG )
{
satRawCOG = dtmp ;
@ -600,14 +639,17 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -600,14 +639,17 @@ void UAS::receiveMessage(mavlink_message_t message)
emit localizationChanged ( this , loc_type ) ;
}
break ;
case MAVLINK_MSG_ID_GPS_STATUS :
{
mavlink_gps_status_t pos ;
mavlink_msg_gps_status_decode ( & message , & pos ) ;
for ( int i = 0 ; i < ( int ) pos . satellites_visible ; i + + )
{
emit gpsSatelliteStatusChanged ( uasId , ( unsigned char ) pos . satellite_prn [ i ] , ( unsigned char ) pos . satellite_elevation [ i ] , ( unsigned char ) pos . satellite_azimuth [ i ] , ( unsigned char ) pos . satellite_snr [ i ] , static_cast < bool > ( pos . satellite_used [ i ] ) ) ;
}
setSatelliteCount ( pos . satellites_visible ) ;
}
break ;
@ -627,6 +669,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -627,6 +669,7 @@ void UAS::receiveMessage(mavlink_message_t message)
processParamValueMsg ( message , parameterName , rawValue , paramVal ) ;
}
break ;
case MAVLINK_MSG_ID_ATTITUDE_TARGET :
{
mavlink_attitude_target_t out ;
@ -649,12 +692,14 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -649,12 +692,14 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break ;
}
mavlink_position_target_local_ned_t p ;
mavlink_msg_position_target_local_ned_decode ( & message , & p ) ;
quint64 time = getUnixTimeFromMs ( p . time_boot_ms ) ;
emit positionSetPointsChanged ( uasId , p . x , p . y , p . z , 0 /* XXX remove yaw and move it to attitude */ , time ) ;
}
break ;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED :
{
mavlink_set_position_target_local_ned_t p ;
@ -662,6 +707,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -662,6 +707,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit userPositionSetPointsChanged ( uasId , p . x , p . y , p . z , 0 /* XXX remove yaw and move it to attitude */ ) ;
}
break ;
case MAVLINK_MSG_ID_STATUSTEXT :
{
QByteArray b ;
@ -681,6 +727,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -681,6 +727,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit textMessageReceived ( uasId , message . compid , severity , text ) ;
_say ( text . toLower ( ) , severity ) ;
}
else
{
emit textMessageReceived ( uasId , message . compid , severity , text ) ;
@ -723,9 +770,11 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -723,9 +770,11 @@ void UAS::receiveMessage(mavlink_message_t message)
for ( int i = 0 ; i < imagePayload ; + + i )
{
if ( pos < = imageSize ) {
if ( pos < = imageSize )
{
imageRecBuffer [ pos ] = img . data [ i ] ;
}
+ + pos ;
}
@ -777,7 +826,8 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -777,7 +826,8 @@ void UAS::receiveMessage(mavlink_message_t message)
void UAS : : startCalibration ( UASInterface : : StartCalibrationType calType )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -788,34 +838,44 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
@@ -788,34 +838,44 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
int accelCal = 0 ;
int escCal = 0 ;
switch ( calType ) {
switch ( calType )
{
case StartCalibrationGyro :
gyroCal = 1 ;
break ;
case StartCalibrationMag :
magCal = 1 ;
break ;
case StartCalibrationAirspeed :
airspeedCal = 1 ;
break ;
case StartCalibrationRadio :
radioCal = 1 ;
break ;
case StartCalibrationCopyTrims :
radioCal = 2 ;
break ;
case StartCalibrationAccel :
accelCal = 1 ;
break ;
case StartCalibrationLevel :
accelCal = 2 ;
break ;
case StartCalibrationEsc :
escCal = 1 ;
break ;
case StartCalibrationUavcanEsc :
escCal = 2 ;
break ;
case StartCalibrationCompassMot :
airspeedCal = 1 ; // ArduPilot, bit of a hack
break ;
@ -841,7 +901,8 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
@@ -841,7 +901,8 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS : : stopCalibration ( void )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -865,16 +926,19 @@ void UAS::stopCalibration(void)
@@ -865,16 +926,19 @@ void UAS::stopCalibration(void)
void UAS : : startBusConfig ( UASInterface : : StartBusConfigType calType )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
int actuatorCal = 0 ;
switch ( calType ) {
switch ( calType )
{
case StartBusConfigActuators :
actuatorCal = 1 ;
break ;
case EndBusConfigActuators :
actuatorCal = 0 ;
break ;
@ -900,7 +964,8 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
@@ -900,7 +964,8 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS : : stopBusConfig ( void )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -936,6 +1001,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -936,6 +1001,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC : : groundTimeMilliseconds ( ) ;
}
// Check if time is smaller than 40 years,
// assuming no system without Unix timestamp
// runs longer than 40 years continuously without
@ -953,6 +1019,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -953,6 +1019,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
# ifndef _MSC_VER
else if ( time < 1261440000000000LLU )
# else
else if ( time < 1261440000000000 )
@ -963,8 +1030,10 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -963,8 +1030,10 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
{
onboardTimeOffset = QGC : : groundTimeMilliseconds ( ) - time / 1000 ;
}
return time / 1000 + onboardTimeOffset ;
}
else
{
// Time is not zero and larger than 40 years -> has to be
@ -999,6 +1068,7 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
@@ -999,6 +1068,7 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
quint64 UAS : : getUnixTime ( quint64 time )
{
quint64 ret = 0 ;
if ( attitudeStamped )
{
ret = lastAttitude ;
@ -1008,6 +1078,7 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1008,6 +1078,7 @@ quint64 UAS::getUnixTime(quint64 time)
{
ret = QGC : : groundTimeMilliseconds ( ) ;
}
// Check if time is smaller than 40 years,
// assuming no system without Unix timestamp
// runs longer than 40 years continuously without
@ -1025,6 +1096,7 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1025,6 +1096,7 @@ quint64 UAS::getUnixTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
# ifndef _MSC_VER
else if ( time < 1261440000000000LLU )
# else
else if ( time < 1261440000000000 )
@ -1036,10 +1108,12 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1036,10 +1108,12 @@ quint64 UAS::getUnixTime(quint64 time)
lastNonNullTime = time ;
onboardTimeOffset = QGC : : groundTimeMilliseconds ( ) - time / 1000 ;
}
if ( time > lastNonNullTime ) lastNonNullTime = time ;
ret = time / 1000 + onboardTimeOffset ;
}
else
{
// Time is not zero and larger than 40 years -> has to be
@ -1063,30 +1137,37 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
@@ -1063,30 +1137,37 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
uasState = tr ( " UNINIT " ) ;
stateDescription = tr ( " Unitialized, booting up. " ) ;
break ;
case MAV_STATE_BOOT :
uasState = tr ( " BOOT " ) ;
stateDescription = tr ( " Booting system, please wait. " ) ;
break ;
case MAV_STATE_CALIBRATING :
uasState = tr ( " CALIBRATING " ) ;
stateDescription = tr ( " Calibrating sensors, please wait. " ) ;
break ;
case MAV_STATE_ACTIVE :
uasState = tr ( " ACTIVE " ) ;
stateDescription = tr ( " Active, normal operation. " ) ;
break ;
case MAV_STATE_STANDBY :
uasState = tr ( " STANDBY " ) ;
stateDescription = tr ( " Standby mode, ready for launch. " ) ;
break ;
case MAV_STATE_CRITICAL :
uasState = tr ( " CRITICAL " ) ;
stateDescription = tr ( " FAILURE: Continuing operation. " ) ;
break ;
case MAV_STATE_EMERGENCY :
uasState = tr ( " EMERGENCY " ) ;
stateDescription = tr ( " EMERGENCY: Land Immediately! " ) ;
break ;
//case MAV_STATE_HILSIM:
//uasState = tr("HIL SIM");
//stateDescription = tr("HIL Simulation, Sensors read from SIM");
@ -1136,6 +1217,7 @@ QImage UAS::getImage()
@@ -1136,6 +1217,7 @@ QImage UAS::getImage()
}
}
// BMP with header
else if ( imageType = = MAVLINK_DATA_STREAM_IMG_BMP | |
imageType = = MAVLINK_DATA_STREAM_IMG_JPEG | |
@ -1158,7 +1240,8 @@ QImage UAS::getImage()
@@ -1158,7 +1240,8 @@ QImage UAS::getImage()
void UAS : : requestImage ( )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1187,6 +1270,7 @@ quint64 UAS::getUptime() const
@@ -1187,6 +1270,7 @@ quint64 UAS::getUptime() const
{
return 0 ;
}
else
{
return QGC : : groundTimeMilliseconds ( ) - startTime ;
@ -1202,7 +1286,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
@@ -1202,7 +1286,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
// Insert with correct type
switch ( rawValue . param_type ) {
switch ( rawValue . param_type )
{
case MAV_PARAM_TYPE_REAL32 :
paramValue = QVariant ( paramUnion . param_float ) ;
break ;
@ -1250,7 +1335,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
@@ -1250,7 +1335,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
void UAS : : executeCommand ( MAV_CMD command , int confirmation , float param1 , float param2 , float param3 , float param4 , float param5 , float param6 , float param7 , int component )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1277,7 +1363,8 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
@@ -1277,7 +1363,8 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
*/
void UAS : : setExternalControlSetpoint ( float roll , float pitch , float yaw , float thrust , quint16 buttons , int joystickMode )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1295,12 +1382,17 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1295,12 +1382,17 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
// The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
bool sendCommand = false ;
if ( countSinceLastTransmission + + > = 5 ) {
if ( countSinceLastTransmission + + > = 5 )
{
sendCommand = true ;
countSinceLastTransmission = 0 ;
} else if ( ( ! qIsNaN ( roll ) & & roll ! = manualRollAngle ) | | ( ! qIsNaN ( pitch ) & & pitch ! = manualPitchAngle ) | |
}
else if ( ( ! qIsNaN ( roll ) & & roll ! = manualRollAngle ) | | ( ! qIsNaN ( pitch ) & & pitch ! = manualPitchAngle ) | |
( ! qIsNaN ( yaw ) & & yaw ! = manualYawAngle ) | | ( ! qIsNaN ( thrust ) & & thrust ! = manualThrust ) | |
buttons ! = manualButtons ) {
buttons ! = manualButtons )
{
sendCommand = true ;
// Ensure that another message will be sent the next time this function is called
@ -1308,7 +1400,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1308,7 +1400,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
}
// Now if we should trigger an update, let's do that
if ( sendCommand ) {
if ( sendCommand )
{
// Save the new manual control inputs
manualRollAngle = roll ;
manualPitchAngle = pitch ;
@ -1318,7 +1411,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1318,7 +1411,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
mavlink_message_t message ;
if ( joystickMode = = Vehicle : : JoystickModeAttitude ) {
if ( joystickMode = = Vehicle : : JoystickModeAttitude )
{
// send an external attitude setpoint command (rate control disabled)
float attitudeQuaternion [ 4 ] ;
mavlink_euler_to_quaternion ( roll , pitch , yaw , attitudeQuaternion ) ;
@ -1336,7 +1430,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1336,7 +1430,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0 ,
thrust
) ;
} else if ( joystickMode = = Vehicle : : JoystickModePosition ) {
}
else if ( joystickMode = = Vehicle : : JoystickModePosition )
{
// Send the the local position setpoint (local pos sp external message)
static float px = 0 ;
static float py = 0 ;
@ -1366,7 +1463,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1366,7 +1463,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
yaw ,
0
) ;
} else if ( joystickMode = = Vehicle : : JoystickModeForce ) {
}
else if ( joystickMode = = Vehicle : : JoystickModeForce )
{
// Send the the force setpoint (local pos sp external message)
float dcm [ 3 ] [ 3 ] ;
mavlink_euler_to_dcm ( roll , pitch , yaw , dcm ) ;
@ -1394,7 +1494,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1394,7 +1494,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0 ,
0
) ;
} else if ( joystickMode = = Vehicle : : JoystickModeVelocity ) {
}
else if ( joystickMode = = Vehicle : : JoystickModeVelocity )
{
// Send the the local velocity setpoint (local pos sp external message)
static float vx = 0 ;
static float vy = 0 ;
@ -1426,7 +1529,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1426,7 +1529,10 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0 ,
yawrate
) ;
} else if ( joystickMode = = Vehicle : : JoystickModeRC ) {
}
else if ( joystickMode = = Vehicle : : JoystickModeRC )
{
// Save the new manual control inputs
manualRollAngle = roll ;
@ -1460,9 +1566,11 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1460,9 +1566,11 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
# ifndef __mobile__
void UAS : : setManual6DOFControlCommands ( double x , double y , double z , double roll , double pitch , double yaw )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
const uint8_t base_mode = _vehicle - > baseMode ( ) ;
// If system has manual inputs enabled and is armed
@ -1491,6 +1599,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
@@ -1491,6 +1599,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
//emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
}
else
{
qDebug ( ) < < " 3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first " ;
@ -1503,7 +1612,8 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
@@ -1503,7 +1612,8 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
*/
void UAS : : pairRX ( int rxType , int rxSubType )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1522,12 +1632,16 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1522,12 +1632,16 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
Q_UNUSED ( configuration ) ;
QGCFlightGearLink * link = dynamic_cast < QGCFlightGearLink * > ( simulation ) ;
if ( ! link ) {
if ( ! link )
{
// Delete wrong sim
if ( simulation ) {
if ( simulation )
{
stopHil ( ) ;
delete simulation ;
}
simulation = new QGCFlightGearLink ( _vehicle , options ) ;
}
@ -1550,12 +1664,14 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1550,12 +1664,14 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
link = dynamic_cast < QGCFlightGearLink * > ( simulation ) ;
link - > setStartupArguments ( options ) ;
link - > sensorHilEnabled ( sensorHil ) ;
// FIXME: this signal is not on the base hil configuration widget, only on the FG widget
//QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float)));
if ( enable )
{
startHil ( ) ;
}
else
{
stopHil ( ) ;
@ -1570,21 +1686,28 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1570,21 +1686,28 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
void UAS : : enableHilJSBSim ( bool enable , QString options )
{
QGCJSBSimLink * link = dynamic_cast < QGCJSBSimLink * > ( simulation ) ;
if ( ! link ) {
if ( ! link )
{
// Delete wrong sim
if ( simulation ) {
if ( simulation )
{
stopHil ( ) ;
delete simulation ;
}
simulation = new QGCJSBSimLink ( _vehicle , options ) ;
}
// Connect Flight Gear Link
link = dynamic_cast < QGCJSBSimLink * > ( simulation ) ;
link - > setStartupArguments ( options ) ;
if ( enable )
{
startHil ( ) ;
}
else
{
stopHil ( ) ;
@ -1599,11 +1722,15 @@ void UAS::enableHilJSBSim(bool enable, QString options)
@@ -1599,11 +1722,15 @@ void UAS::enableHilJSBSim(bool enable, QString options)
void UAS : : enableHilXPlane ( bool enable )
{
QGCXPlaneLink * link = dynamic_cast < QGCXPlaneLink * > ( simulation ) ;
if ( ! link ) {
if ( simulation ) {
if ( ! link )
{
if ( simulation )
{
stopHil ( ) ;
delete simulation ;
}
simulation = new QGCXPlaneLink ( _vehicle ) ;
float noise_scaler = 0.0001f ;
@ -1621,11 +1748,13 @@ void UAS::enableHilXPlane(bool enable)
@@ -1621,11 +1748,13 @@ void UAS::enableHilXPlane(bool enable)
pressure_alt_var = noise_scaler * 0.5604f ;
temperature_var = noise_scaler * 0.7290f ;
}
// Connect X-Plane Link
if ( enable )
{
startHil ( ) ;
}
else
{
stopHil ( ) ;
@ -1706,7 +1835,8 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
@@ -1706,7 +1835,8 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
float pitchspeed , float yawspeed , double lat , double lon , double alt ,
float vx , float vy , float vz , float ind_airspeed , float true_airspeed , float xacc , float yacc , float zacc )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1735,6 +1865,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
@@ -1735,6 +1865,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
lat * 1e7 f , lon * 1e7 f , alt * 1000 , vx * 100 , vy * 100 , vz * 100 , ind_airspeed * 100 , true_airspeed * 100 , xacc * 1000 / 9.81 , yacc * 1000 / 9.81 , zacc * 1000 / 9.81 ) ;
_vehicle - > sendMessageOnPriorityLink ( msg ) ;
}
else
{
// Attempt to set HIL mode
@ -1770,9 +1901,13 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
@@ -1770,9 +1901,13 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
float noise = z0 * sqrt ( noise_var ) ; //calculate normally distributed variable with mu = 0, std = var^2
//Finally guard against any case where the noise is not real
if ( std : : isfinite ( noise ) ) {
if ( std : : isfinite ( noise ) )
{
return truth_meas + noise ;
} else {
}
else
{
return truth_meas ;
}
}
@ -1786,7 +1921,8 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
@@ -1786,7 +1921,8 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
void UAS : : sendHilSensors ( quint64 time_us , float xacc , float yacc , float zacc , float rollspeed , float pitchspeed , float yawspeed ,
float xmag , float ymag , float zmag , float abs_pressure , float diff_pressure , float pressure_alt , float temperature , quint32 fields_changed )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1814,6 +1950,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
@@ -1814,6 +1950,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
_vehicle - > sendMessageOnPriorityLink ( msg ) ;
lastSendTimeSensors = QGC : : groundTimeMilliseconds ( ) ;
}
else
{
// Attempt to set HIL mode
@ -1827,7 +1964,8 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
@@ -1827,7 +1964,8 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
void UAS : : sendHilOpticalFlow ( quint64 time_us , qint16 flow_x , qint16 flow_y , float flow_comp_m_x ,
float flow_comp_m_y , quint8 quality , float ground_distance )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1852,6 +1990,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
@@ -1852,6 +1990,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
lastSendTimeOpticalFlow = QGC : : groundTimeMilliseconds ( ) ;
# endif
}
else
{
// Attempt to set HIL mode
@ -1865,7 +2004,8 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
@@ -1865,7 +2004,8 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
# ifndef __mobile__
void UAS : : sendHilGps ( quint64 time_us , double lat , double lon , double alt , int fix_type , float eph , float epv , float vel , float vn , float ve , float vd , float cog , int satellites )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
@ -1876,9 +2016,11 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -1876,9 +2016,11 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
if ( _vehicle - > hilMode ( ) )
{
float course = cog ;
// map to 0..2pi
if ( course < 0 )
course + = 2.0f * static_cast < float > ( M_PI ) ;
// scale from radians to degrees
course = ( course / M_PI ) * 180.0f ;
@ -1888,6 +2030,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -1888,6 +2030,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
lastSendTimeGPS = QGC : : groundTimeMilliseconds ( ) ;
_vehicle - > sendMessageOnPriorityLink ( msg ) ;
}
else
{
// Attempt to set HIL mode
@ -1904,6 +2047,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -1904,6 +2047,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
void UAS : : startHil ( )
{
if ( hilEnabled ) return ;
hilEnabled = true ;
sensorHil = false ;
_vehicle - > setHilMode ( true ) ;
@ -1919,11 +2063,13 @@ void UAS::startHil()
@@ -1919,11 +2063,13 @@ void UAS::startHil()
# ifndef __mobile__
void UAS : : stopHil ( )
{
if ( simulation & & simulation - > isConnected ( ) ) {
if ( simulation & & simulation - > isConnected ( ) )
{
simulation - > disconnectSimulation ( ) ;
_vehicle - > setHilMode ( false ) ;
qDebug ( ) < < __FILE__ < < __LINE__ < < " HIL is onboard not enabled, trying to disable. " ;
}
hilEnabled = false ;
sensorHil = false ;
}
@ -1939,13 +2085,15 @@ QMap<int, QString> UAS::getComponents()
@@ -1939,13 +2085,15 @@ QMap<int, QString> UAS::getComponents()
void UAS : : sendMapRCToParam ( QString param_id , float scale , float value0 , quint8 param_rc_channel_index , float valueMin , float valueMax )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
mavlink_message_t message ;
char param_id_cstr [ MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN ] = { } ;
// Copy string into buffer, ensuring not to exceed the buffer size
for ( unsigned int i = 0 ; i < sizeof ( param_id_cstr ) ; i + + )
{
@ -1973,13 +2121,15 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
@@ -1973,13 +2121,15 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
void UAS : : unsetRCToParameterMap ( )
{
if ( ! _vehicle ) {
if ( ! _vehicle )
{
return ;
}
char param_id_cstr [ MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN ] = { } ;
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int i = 0 ; i < 3 ; i + + )
{
mavlink_message_t message ;
mavlink_msg_param_map_rc_pack ( mavlink - > getSystemId ( ) ,
mavlink - > getComponentId ( ) ,
@ -2007,12 +2157,15 @@ void UAS::shutdownVehicle(void)
@@ -2007,12 +2157,15 @@ void UAS::shutdownVehicle(void)
{
# ifndef __mobile__
stopHil ( ) ;
if ( simulation ) {
if ( simulation )
{
// wait for the simulator to exit
simulation - > wait ( ) ;
simulation - > disconnectSimulation ( ) ;
simulation - > deleteLater ( ) ;
}
# endif
_vehicle = NULL ;
}