@ -201,19 +201,16 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -201,19 +201,16 @@ 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 " ;
@ -243,7 +240,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -243,7 +240,6 @@ 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 ;
@ -255,7 +251,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -255,7 +251,6 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer the first component
componentID [ message . msgid ] = message . compid ;
}
else
{
// Got this message already
@ -277,7 +272,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -277,7 +272,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break ;
}
mavlink_heartbeat_t state ;
mavlink_msg_heartbeat_decode ( & message , & state ) ;
@ -309,7 +303,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -309,7 +303,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break ;
}
mavlink_sys_status_t state ;
mavlink_msg_sys_status_decode ( & message , & state ) ;
@ -343,12 +336,10 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -343,12 +336,10 @@ 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 ;
@ -370,7 +361,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -370,7 +361,6 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION :
{
mavlink_attitude_quaternion_t attitude ;
@ -400,24 +390,17 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -400,24 +390,17 @@ 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 ] ) ;
}
@ -439,7 +422,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -439,7 +422,6 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_HIL_CONTROLS :
{
mavlink_hil_controls_t hil ;
@ -447,7 +429,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -447,7 +429,6 @@ 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 ;
@ -472,7 +453,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -472,7 +453,6 @@ void UAS::receiveMessage(mavlink_message_t message)
hil . mode ) ;
}
break ;
case MAVLINK_MSG_ID_VFR_HUD :
{
mavlink_vfr_hud_t hud ;
@ -489,16 +469,13 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -489,16 +469,13 @@ 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;
@ -518,7 +495,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -518,7 +495,6 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break ;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE :
{
mavlink_global_vision_position_estimate_t pos ;
@ -527,7 +503,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -527,7 +503,6 @@ 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;
@ -558,7 +533,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -558,7 +533,6 @@ void UAS::receiveMessage(mavlink_message_t message)
isGlobalPositionKnown = true ;
}
break ;
case MAVLINK_MSG_ID_GPS_RAW_INT :
{
mavlink_gps_raw_int_t pos ;
@ -568,12 +542,10 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -568,12 +542,10 @@ 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 )
@ -585,24 +557,18 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -585,24 +557,18 @@ 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 ) ) ;
}
}
@ -611,23 +577,18 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -611,23 +577,18 @@ 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 ;
@ -639,17 +600,14 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -639,17 +600,14 @@ 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 ;
@ -669,7 +627,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -669,7 +627,6 @@ void UAS::receiveMessage(mavlink_message_t message)
processParamValueMsg ( message , parameterName , rawValue , paramVal ) ;
}
break ;
case MAVLINK_MSG_ID_ATTITUDE_TARGET :
{
mavlink_attitude_target_t out ;
@ -692,14 +649,12 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -692,14 +649,12 @@ 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 ;
@ -707,7 +662,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -707,7 +662,6 @@ 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 ;
@ -727,7 +681,6 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -727,7 +681,6 @@ 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 ) ;
@ -770,11 +723,9 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -770,11 +723,9 @@ 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 ;
}
@ -826,8 +777,7 @@ void UAS::receiveMessage(mavlink_message_t message)
@@ -826,8 +777,7 @@ void UAS::receiveMessage(mavlink_message_t message)
void UAS : : startCalibration ( UASInterface : : StartCalibrationType calType )
{
if ( ! _vehicle )
{
if ( ! _vehicle ) {
return ;
}
@ -838,44 +788,34 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
@@ -838,44 +788,34 @@ 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 ;
@ -901,8 +841,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
@@ -901,8 +841,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS : : stopCalibration ( void )
{
if ( ! _vehicle )
{
if ( ! _vehicle ) {
return ;
}
@ -926,19 +865,16 @@ void UAS::stopCalibration(void)
@@ -926,19 +865,16 @@ 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 ;
@ -964,8 +900,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
@@ -964,8 +900,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS : : stopBusConfig ( void )
{
if ( ! _vehicle )
{
if ( ! _vehicle ) {
return ;
}
@ -1001,7 +936,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -1001,7 +936,6 @@ 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
@ -1019,7 +953,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -1019,7 +953,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
# ifndef _MSC_VER
else if ( time < 1261440000000000LLU )
# else
else if ( time < 1261440000000000 )
@ -1030,10 +963,8 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
@@ -1030,10 +963,8 @@ 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
@ -1068,7 +999,6 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
@@ -1068,7 +999,6 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
quint64 UAS : : getUnixTime ( quint64 time )
{
quint64 ret = 0 ;
if ( attitudeStamped )
{
ret = lastAttitude ;
@ -1078,7 +1008,6 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1078,7 +1008,6 @@ 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
@ -1096,7 +1025,6 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1096,7 +1025,6 @@ quint64 UAS::getUnixTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
# ifndef _MSC_VER
else if ( time < 1261440000000000LLU )
# else
else if ( time < 1261440000000000 )
@ -1108,12 +1036,10 @@ quint64 UAS::getUnixTime(quint64 time)
@@ -1108,12 +1036,10 @@ 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
@ -1137,37 +1063,30 @@ void UAS::getStatusForCode(int statusCode, QString &uasState, QString &stateDesc
@@ -1137,37 +1063,30 @@ 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");
@ -1217,7 +1136,6 @@ QImage UAS::getImage()
@@ -1217,7 +1136,6 @@ QImage UAS::getImage()
}
}
// BMP with header
else if ( imageType = = MAVLINK_DATA_STREAM_IMG_BMP | |
imageType = = MAVLINK_DATA_STREAM_IMG_JPEG | |
@ -1240,8 +1158,7 @@ QImage UAS::getImage()
@@ -1240,8 +1158,7 @@ QImage UAS::getImage()
void UAS : : requestImage ( )
{
if ( ! _vehicle )
{
if ( ! _vehicle ) {
return ;
}
@ -1270,7 +1187,6 @@ quint64 UAS::getUptime() const
@@ -1270,7 +1187,6 @@ quint64 UAS::getUptime() const
{
return 0 ;
}
else
{
return QGC : : groundTimeMilliseconds ( ) - startTime ;
@ -1286,8 +1202,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString ¶mName,
@@ -1286,8 +1202,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString ¶mName,
// Insert with correct type
switch ( rawValue . param_type )
{
switch ( rawValue . param_type ) {
case MAV_PARAM_TYPE_REAL32 :
paramValue = QVariant ( paramUnion . param_float ) ;
break ;
@ -1335,8 +1250,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString ¶mName,
@@ -1335,8 +1250,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString ¶mName,
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 ;
}
@ -1363,8 +1277,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
@@ -1363,8 +1277,7 @@ 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 ;
}
@ -1382,17 +1295,12 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1382,17 +1295,12 @@ 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
@ -1400,8 +1308,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1400,8 +1308,7 @@ 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 ;
@ -1411,8 +1318,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1411,8 +1318,7 @@ 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 ) ;
@ -1430,10 +1336,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1430,10 +1336,7 @@ 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 ;
@ -1463,10 +1366,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1463,10 +1366,7 @@ 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 ) ;
@ -1494,10 +1394,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1494,10 +1394,7 @@ 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 ;
@ -1529,10 +1426,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1529,10 +1426,7 @@ 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 ;
@ -1566,11 +1460,9 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
@@ -1566,11 +1460,9 @@ 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
@ -1599,7 +1491,6 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
@@ -1599,7 +1491,6 @@ 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 " ;
@ -1612,8 +1503,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
@@ -1612,8 +1503,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
*/
void UAS : : pairRX ( int rxType , int rxSubType )
{
if ( ! _vehicle )
{
if ( ! _vehicle ) {
return ;
}
@ -1632,16 +1522,12 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1632,16 +1522,12 @@ 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 ) ;
}
@ -1664,14 +1550,12 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1664,14 +1550,12 @@ 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 ( ) ;
@ -1686,28 +1570,21 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
@@ -1686,28 +1570,21 @@ 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 ( ) ;
@ -1722,15 +1599,11 @@ void UAS::enableHilJSBSim(bool enable, QString options)
@@ -1722,15 +1599,11 @@ 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 ;
@ -1748,13 +1621,11 @@ void UAS::enableHilXPlane(bool enable)
@@ -1748,13 +1621,11 @@ 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 ( ) ;
@ -1835,8 +1706,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
@@ -1835,8 +1706,7 @@ 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 ;
}
@ -1865,7 +1735,6 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
@@ -1865,7 +1735,6 @@ 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
@ -1901,13 +1770,9 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
@@ -1901,13 +1770,9 @@ 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 ;
}
}
@ -1921,8 +1786,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
@@ -1921,8 +1786,7 @@ 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 ;
}
@ -1950,7 +1814,6 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
@@ -1950,7 +1814,6 @@ 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
@ -1964,8 +1827,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
@@ -1964,8 +1827,7 @@ 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 ;
}
@ -1990,7 +1852,6 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
@@ -1990,7 +1852,6 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
lastSendTimeOpticalFlow = QGC : : groundTimeMilliseconds ( ) ;
# endif
}
else
{
// Attempt to set HIL mode
@ -2004,8 +1865,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
@@ -2004,8 +1865,7 @@ 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 ;
}
@ -2016,11 +1876,9 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -2016,11 +1876,9 @@ 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 ;
@ -2030,7 +1888,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -2030,7 +1888,6 @@ 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
@ -2047,7 +1904,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
@@ -2047,7 +1904,6 @@ 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 ) ;
@ -2063,13 +1919,11 @@ void UAS::startHil()
@@ -2063,13 +1919,11 @@ 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 ;
}
@ -2085,15 +1939,13 @@ QMap<int, QString> UAS::getComponents()
@@ -2085,15 +1939,13 @@ 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 + + )
{
@ -2121,15 +1973,13 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
@@ -2121,15 +1973,13 @@ 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 ( ) ,
@ -2157,15 +2007,12 @@ void UAS::shutdownVehicle(void)
@@ -2157,15 +2007,12 @@ 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 ;
}