Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
8547fa05
Commit
8547fa05
authored
Feb 24, 2012
by
LM
Browse files
Options
Browse Files
Download
Plain Diff
Fixed UART baud rates. 921600 IS supported on all platforms, they just do not define / report it
parents
18340b64
821e0e63
Changes
11
Hide whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
982 additions
and
880 deletions
+982
-880
qgroundcontrol.pro
qgroundcontrol.pro
+6
-6
SerialLink.cc
src/comm/SerialLink.cc
+108
-90
UAS.cc
src/uas/UAS.cc
+68
-8
UAS.h
src/uas/UAS.h
+5
-0
HSIDisplay.cc
src/ui/HSIDisplay.cc
+1
-7
MainWindow.cc
src/ui/MainWindow.cc
+8
-7
SerialConfigurationWindow.cc
src/ui/SerialConfigurationWindow.cc
+46
-30
QGCMAVLinkMessageSender.cc
src/ui/mavlink/QGCMAVLinkMessageSender.cc
+1
-1
qportsettings.h
thirdParty/qserialport/include/QtSerialPort/qportsettings.h
+167
-163
qportsettings.cpp
thirdParty/qserialport/src/common/qportsettings.cpp
+186
-186
termioshelper.cpp
thirdParty/qserialport/src/posix/termioshelper.cpp
+386
-382
No files found.
qgroundcontrol.pro
View file @
8547fa05
...
...
@@ -363,8 +363,7 @@ HEADERS += src/MG.h \
src
/
ui
/
mavlink
/
QGCMAVLinkMessageSender
.
h
\
src
/
ui
/
firmwareupdate
/
QGCFirmwareUpdateWidget
.
h
\
src
/
ui
/
QGCPluginHost
.
h
\
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
h
\
src
/
ui
/
map3D
/
TerrainParamDialog
.
h
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
h
#
Google
Earth
is
only
supported
on
Mac
OS
and
Windows
with
Visual
Studio
Compiler
macx
|
macx
-
g
++
|
macx
-
g
++
42
|
win32
-
msvc2008
|
win32
-
msvc2010
::
HEADERS
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
h
...
...
@@ -391,7 +390,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src
/
ui
/
map3D
/
Texture
.
h
\
src
/
ui
/
map3D
/
Imagery
.
h
\
src
/
ui
/
map3D
/
HUDScaleGeode
.
h
\
src
/
ui
/
map3D
/
WaypointGroupNode
.
h
src
/
ui
/
map3D
/
WaypointGroupNode
.
h
\
src
/
ui
/
map3D
/
TerrainParamDialog
.
h
}
contains
(
DEPENDENCIES_PRESENT
,
protobuf
)
:
contains
(
MAVLINK_CONF
,
pixhawk
)
{
message
(
"Including headers for Protocol Buffers"
)
...
...
@@ -504,8 +504,7 @@ SOURCES += src/main.cc \
src
/
ui
/
mavlink
/
QGCMAVLinkMessageSender
.
cc
\
src
/
ui
/
firmwareupdate
/
QGCFirmwareUpdateWidget
.
cc
\
src
/
ui
/
QGCPluginHost
.
cc
\
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
cc
\
src
/
ui
/
map3D
/
TerrainParamDialog
.
cc
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
cc
#
Enable
Google
Earth
only
on
Mac
OS
and
Windows
with
Visual
Studio
compiler
macx
|
macx
-
g
++
|
macx
-
g
++
42
|
win32
-
msvc2008
|
win32
-
msvc2010
::
SOURCES
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
cc
...
...
@@ -534,7 +533,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src
/
ui
/
map3D
/
Texture
.
cc
\
src
/
ui
/
map3D
/
Imagery
.
cc
\
src
/
ui
/
map3D
/
HUDScaleGeode
.
cc
\
src
/
ui
/
map3D
/
WaypointGroupNode
.
cc
src
/
ui
/
map3D
/
WaypointGroupNode
.
cc
\
src
/
ui
/
map3D
/
TerrainParamDialog
.
cc
contains
(
DEPENDENCIES_PRESENT
,
osgearth
)
{
message
(
"Including sources for osgEarth"
)
...
...
src/comm/SerialLink.cc
View file @
8547fa05
...
...
@@ -645,20 +645,23 @@ void SerialLink::setName(QString name)
}
/**
* This function maps baud rate constants to numerical equivalents.
* It relies on the mapping given in qportsettings.h from the QSerialPort library.
*/
qint64
SerialLink
::
getNominalDataRate
()
{
qint64
dataRate
=
0
;
switch
(
portSettings
.
baudRate
())
{
#ifndef Q_OS_WIN
// Baud rates supported only by POSIX systems
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
case
QPortSettings
:
:
BAUDR_50
:
dataRate
=
50
;
break
;
case
QPortSettings
:
:
BAUDR_75
:
dataRate
=
75
;
break
;
case
QPortSettings
:
:
BAUDR_110
:
dataRate
=
110
;
break
;
case
QPortSettings
:
:
BAUDR_134
:
dataRate
=
134
;
break
;
...
...
@@ -668,7 +671,48 @@ qint64 SerialLink::getNominalDataRate()
case
QPortSettings
:
:
BAUDR_200
:
dataRate
=
200
;
break
;
case
QPortSettings
:
:
BAUDR_1800
:
dataRate
=
1800
;
break
;
#endif
// Baud rates supported only by Linux
#ifdef Q_OS_LINUX
case
QPortSettings
:
:
BAUDR_230400
:
dataRate
=
230400
;
break
;
case
QPortSettings
:
:
BAUDR_460800
:
dataRate
=
460800
;
break
;
case
QPortSettings
:
:
BAUDR_500000
:
dataRate
=
500000
;
break
;
case
QPortSettings
:
:
BAUDR_576000
:
dataRate
=
576000
;
break
;
case
QPortSettings
:
:
BAUDR_921600
:
dataRate
=
921600
;
break
;
#endif
// Baud rates supported only by Windows
#if defined(Q_OS_WIN32) || defined(Q_OS_WINCE)
case
QPortSettings
:
:
BAUDR_14400
:
dataRate
=
14400
;
break
;
case
QPortSettings
:
:
BAUDR_56000
:
dataRate
=
56000
;
break
;
case
QPortSettings
:
:
BAUDR_128000
:
dataRate
=
128000
;
break
;
case
QPortSettings
:
:
BAUDR_256000
:
dataRate
=
256000
;
#endif
case
QPortSettings
:
:
BAUDR_110
:
dataRate
=
110
;
break
;
case
QPortSettings
:
:
BAUDR_300
:
dataRate
=
300
;
break
;
...
...
@@ -678,11 +722,6 @@ qint64 SerialLink::getNominalDataRate()
case
QPortSettings
:
:
BAUDR_1200
:
dataRate
=
1200
;
break
;
#ifndef Q_OS_WIN
case
QPortSettings
:
:
BAUDR_1800
:
dataRate
=
1800
;
break
;
#endif
case
QPortSettings
:
:
BAUDR_2400
:
dataRate
=
2400
;
break
;
...
...
@@ -692,52 +731,22 @@ qint64 SerialLink::getNominalDataRate()
case
QPortSettings
:
:
BAUDR_9600
:
dataRate
=
9600
;
break
;
#ifdef Q_OS_WIN
case
QPortSettings
:
:
BAUDR_14400
:
dataRate
=
14400
;
break
;
#endif
case
QPortSettings
:
:
BAUDR_19200
:
dataRate
=
19200
;
break
;
case
QPortSettings
:
:
BAUDR_38400
:
dataRate
=
38400
;
break
;
#ifdef Q_OS_WIN
case
QPortSettings
:
:
BAUDR_56000
:
dataRate
=
56000
;
break
;
#endif
case
QPortSettings
:
:
BAUDR_57600
:
dataRate
=
57600
;
break
;
#ifdef Q_OS_WIN_XXXX // FIXME
case
QPortSettings
:
:
BAUDR_76800
:
dataRate
=
76800
;
break
;
#endif
case
QPortSettings
:
:
BAUDR_115200
:
dataRate
=
115200
;
break
;
#ifdef Q_OS_WIN
// Windows-specific high-end baudrates
case
QPortSettings
:
:
BAUDR_128000
:
dataRate
=
128000
;
break
;
case
QPortSettings
:
:
BAUDR_256000
:
dataRate
=
256000
;
case
QPortSettings
:
:
BAUDR_230400
:
dataRate
=
230400
;
case
QPortSettings
:
:
BAUDR_460800
:
dataRate
=
460800
;
#endif
// All-OS high-speed
case
QPortSettings
:
:
BAUDR_921600
:
dataRate
=
921600
;
break
;
// Otherwise do nothing.
case
QPortSettings
:
:
BAUDR_UNKNOWN
:
default:
// Do nothing
default:
break
;
}
return
dataRate
;
...
...
@@ -907,13 +916,19 @@ bool SerialLink::setBaudRateType(int rateIndex)
if
(
isConnected
())
reconnect
=
true
;
disconnect
();
#ifdef Q_OS_WIN
const
int
minBaud
=
(
int
)
QPortSettings
::
BAUDR_14400
;
#else
// These minimum and maximum baud rates were based on those enumerated in qportsettings.h.
#if defined(Q_OS_WIN32) || defined(Q_OS_WINCE)
const
int
minBaud
=
(
int
)
QPortSettings
::
BAUDR_110
;
const
int
maxBaud
=
(
int
)
QPortSettings
::
BAUDR_256000
;
#elif defined(Q_OS_LINUX)
const
int
minBaud
=
(
int
)
QPortSettings
::
BAUDR_50
;
const
int
maxBaud
=
(
int
)
QPortSettings
::
BAUDR_921600
;
#elif defined(Q_OS_UNIX) || defined(Q_OS_DARWIN)
const
int
minBaud
=
(
int
)
QPortSettings
::
BAUDR_50
;
const
int
maxBaud
=
(
int
)
QPortSettings
::
BAUDR_115200
;
#endif
if
(
rateIndex
>=
minBaud
&&
rateIndex
<=
(
int
)
QPortSettings
::
BAUDR_921600
)
if
(
rateIndex
>=
minBaud
&&
rateIndex
<=
maxBaud
)
{
portSettings
.
setBaudRate
((
QPortSettings
::
BaudRate
)
rateIndex
);
}
...
...
@@ -932,27 +947,24 @@ bool SerialLink::setBaudRateString(const QString& rate)
bool
SerialLink
::
setBaudRate
(
int
rate
)
{
//qDebug() << "BAUD RATE:" << rate;
bool
reconnect
=
false
;
bool
accepted
=
true
;
// This is changed if none of the data rates matches
if
(
isConnected
())
{
reconnect
=
true
;
}
disconnect
();
// This switch-statment relies on the mapping given in qportsettings.h from the QSerialPort library.
switch
(
rate
)
{
#ifndef Q_OS_WIN
// Baud rates supported only by non-Windows systems
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
case
50
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_50
);
break
;
case
75
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_75
);
break
;
case
110
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_110
);
break
;
case
134
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_134
);
break
;
...
...
@@ -962,7 +974,50 @@ bool SerialLink::setBaudRate(int rate)
case
200
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_200
);
break
;
case
1800
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_1800
);
break
;
#endif
// Supported only by Linux
#ifdef Q_OS_LINUX
case
230400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_230400
);
break
;
case
460800
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_460800
);
break
;
case
500000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_500000
);
break
;
case
576000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_576000
);
break
;
case
921600
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_921600
);
break
;
#endif
// Baud rates supported only by windows
#if defined(Q_OS_WIN32) || defined(Q_OS_WINCE)
case
14400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_14400
);
break
;
case
56000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_56000
);
break
;
case
128000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_128000
);
break
;
case
256000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_256000
);
break
;
#endif
// Supported by all OSes:
case
110
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_110
);
break
;
case
300
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_300
);
break
;
...
...
@@ -972,11 +1027,6 @@ bool SerialLink::setBaudRate(int rate)
case
1200
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_1200
);
break
;
#ifndef Q_OS_WIN
case
1800
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_1800
);
break
;
#endif
case
2400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_2400
);
break
;
...
...
@@ -986,50 +1036,18 @@ bool SerialLink::setBaudRate(int rate)
case
9600
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_9600
);
break
;
#ifdef Q_OS_WIN
case
14400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_14400
);
break
;
#endif
case
19200
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_19200
);
break
;
case
38400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_38400
);
break
;
#ifdef Q_OS_WIN
case
56000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_56000
);
break
;
#endif
case
57600
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_57600
);
break
;
#ifdef Q_OS_WIN_XXXX // FIXME CHECK THIS
case
76800
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_76800
);
break
;
#endif
case
115200
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_115200
);
break
;
#ifdef Q_OS_WIN
case
128000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_128000
);
break
;
case
230400
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_230400
);
break
;
case
256000
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_256000
);
break
;
case
460800
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_460800
);
break
;
#endif
case
921600
:
portSettings
.
setBaudRate
(
QPortSettings
::
BAUDR_921600
);
break
;
default:
// If none of the above cases matches, there must be an error
accepted
=
false
;
...
...
src/uas/UAS.cc
View file @
8547fa05
...
...
@@ -43,10 +43,13 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
waypointManager
(
this
),
thrustSum
(
0
),
thrustMax
(
10
),
startVoltage
(
0
),
startVoltage
(
-
1.0
f
),
tickVoltage
(
10.5
f
),
lastTickVoltageValue
(
13.0
f
),
tickLowpassVoltage
(
12.0
f
),
warnVoltage
(
9.5
f
),
warnLevelPercent
(
20.0
f
),
currentVoltage
(
12.
0
f
),
currentVoltage
(
12.
6
f
),
lpVoltage
(
12.0
f
),
batteryRemainingEstimateEnabled
(
true
),
mode
(
-
1
),
...
...
@@ -333,15 +336,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
}
QString
audiostring
=
"System "
+
getUASName
(
);
QString
audiostring
=
QString
(
"System %1"
).
arg
(
uasId
);
QString
stateAudio
=
""
;
QString
modeAudio
=
""
;
QString
navModeAudio
=
""
;
bool
statechanged
=
false
;
bool
modechanged
=
false
;
QString
audiomodeText
=
getAudioModeTextFor
(
static_cast
<
int
>
(
state
.
base_mode
));
if
(
state
.
system_status
!=
this
->
status
)
if
((
state
.
system_status
!=
this
->
status
)
&&
state
.
system_status
!=
MAV_STATE_UNINIT
)
{
statechanged
=
true
;
this
->
status
=
state
.
system_status
;
...
...
@@ -362,7 +367,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
modeChanged
(
this
->
getUASID
(),
shortModeText
,
""
);
modeAudio
=
" is now in "
+
shortM
odeText
;
modeAudio
=
" is now in "
+
audiom
odeText
;
}
if
(
navMode
!=
state
.
custom_mode
)
...
...
@@ -414,7 +419,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
valueChanged
(
uasId
,
name
.
arg
(
"errors_count1"
),
"-"
,
state
.
errors_count1
,
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"errors_count2"
),
"-"
,
state
.
errors_count2
,
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"errors_count3"
),
"-"
,
state
.
errors_count3
,
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"errors_count4"
),
"-"
,
state
.
errors_count4
,
time
);
emit
valueChanged
(
uasId
,
name
.
arg
(
"errors_count4"
),
"-"
,
state
.
errors_count4
,
time
);
// Process CPU load.
emit
loadChanged
(
this
,
state
.
load
/
10.0
f
);
...
...
@@ -423,8 +428,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Battery charge/time remaining/voltage calculations
currentVoltage
=
state
.
voltage_battery
/
1000.0
f
;
lpVoltage
=
filterVoltage
(
currentVoltage
);
tickLowpassVoltage
=
tickLowpassVoltage
*
0.8
f
+
0.2
f
*
currentVoltage
;
// We don't want to tick above the threshold
if
(
tickLowpassVoltage
>
tickVoltage
)
{
lastTickVoltageValue
=
tickLowpassVoltage
;
}
if
((
startVoltage
>
0.0
f
)
&&
(
tickLowpassVoltage
<
tickVoltage
)
&&
(
fabs
(
lastTickVoltageValue
-
tickLowpassVoltage
)
>
0.1
f
)
&&
(
lpVoltage
<
tickVoltage
))
{
GAudioOutput
::
instance
()
->
say
(
QString
(
"voltage warning: %1 volt"
).
arg
(
lpVoltage
,
2
));
lastTickVoltageValue
=
tickLowpassVoltage
;
}
if
(
startVoltage
==
0
)
startVoltage
=
currentVoltage
;
if
(
startVoltage
==
-
1.0
f
&&
currentVoltage
>
0.1
f
)
startVoltage
=
currentVoltage
;
timeRemaining
=
calculateTimeRemaining
();
if
(
!
batteryRemainingEstimateEnabled
&&
chargeLevel
!=
-
1
)
{
...
...
@@ -442,7 +461,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
// LOW BATTERY ALARM
if
(
lpVoltage
<
warnVoltage
)
if
(
lpVoltage
<
warnVoltage
&&
(
startVoltage
>
0.0
f
)
)
{
startLowBattAlarm
();
}
...
...
@@ -451,6 +470,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
stopLowBattAlarm
();
}
qDebug
()
<<
"START"
<<
startVoltage
;
// control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
...
...
@@ -2357,6 +2378,45 @@ const QString& UAS::getShortState() const
return
shortStateText
;
}
QString
UAS
::
getAudioModeTextFor
(
int
id
)
{
QString
mode
;
uint8_t
modeid
=
id
;
// BASE MODE DECODING
if
(
modeid
&
(
uint8_t
)
MAV_MODE_FLAG_DECODE_POSITION_AUTO
)
{
mode
+=
"autonomous"
;
}
else
if
(
modeid
&
(
uint8_t
)
MAV_MODE_FLAG_DECODE_POSITION_GUIDED
)
{
mode
+=
"guided"
;
}
else
if
(
modeid
&
(
uint8_t
)
MAV_MODE_FLAG_DECODE_POSITION_MANUAL
)
{
mode
+=
"manual"
;
}
if
(
modeid
!=
0
)
{
mode
+=
" mode"
;
}
// ARMED STATE DECODING
if
(
modeid
&
(
uint8_t
)
MAV_MODE_FLAG_DECODE_POSITION_SAFETY
)
{
mode
.
append
(
" and armed"
);
}
// HARDWARE IN THE LOOP DECODING
if
(
modeid
&
(
uint8_t
)
MAV_MODE_FLAG_DECODE_POSITION_HIL
)
{
mode
.
append
(
" using hardware in the loop simulation"
);
}
return
mode
;
}
QString
UAS
::
getShortModeTextFor
(
int
id
)
{
QString
mode
;
...
...
src/uas/UAS.h
View file @
8547fa05
...
...
@@ -75,6 +75,8 @@ public:
const
QString
&
getShortMode
()
const
;
/** @brief Translate from mode id to text */
static
QString
getShortModeTextFor
(
int
id
);
/** @brief Translate from mode id to audio text */
static
QString
getAudioModeTextFor
(
int
id
);
/** @brief Get the unique system id */
int
getUASID
()
const
;
/** @brief Get the airframe */
...
...
@@ -211,6 +213,9 @@ protected: //COMMENTS FOR TEST UNIT
float
fullVoltage
;
///< Voltage of the fully charged battery (100%)
float
emptyVoltage
;
///< Voltage of the empty battery (0%)
float
startVoltage
;
///< Voltage at system start
float
tickVoltage
;
///< Voltage where 0.1 V ticks are told
float
lastTickVoltageValue
;
///< The last voltage where a tick was announced
float
tickLowpassVoltage
;
///< Lowpass-filtered voltage for the tick announcement
float
warnVoltage
;
///< Voltage where QGC will start to warn about low battery
float
warnLevelPercent
;
///< Warning level, in percent
double
currentVoltage
;
///< Voltage currently measured
...
...
src/ui/HSIDisplay.cc
View file @
8547fa05
...
...
@@ -910,13 +910,7 @@ void HSIDisplay::sendBodySetPointCoordinates()
// Send the coordinates to the MAV
if
(
uas
)
{
double
dx
=
uiXSetCoordinate
-
uas
->
getLocalX
();
double
dy
=
uiYSetCoordinate
-
uas
->
getLocalY
();
double
dz
=
uiZSetCoordinate
-
uas
->
getLocalZ
();
bool
valid
=
(
sqrt
(
dx
*
dx
+
dy
*
dy
+
dz
*
dz
)
<
200.0
);
//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate);
// if (valid)
// {
uas
->
setLocalPositionSetpoint
(
uiXSetCoordinate
,
uiYSetCoordinate
,
uiZSetCoordinate
,
uiYawSet
);
uas
->
setLocalPositionSetpoint
(
uiXSetCoordinate
,
uiYSetCoordinate
,
uiZSetCoordinate
,
uiYawSet
);
}
}
...
...
src/ui/MainWindow.cc
View file @
8547fa05
...
...
@@ -1211,23 +1211,24 @@ void MainWindow::UASCreated(UASInterface* uas)
QIcon
icon
;
// Set matching icon
switch
(
uas
->
getSystemType
())
{
case
0
:
switch
(
uas
->
getSystemType
())
{
case
MAV_TYPE_GENERIC
:
icon
=
QIcon
(
":/images/mavs/generic.svg"
);
break
;
case
1
:
case
MAV_TYPE_FIXED_WING
:
icon
=
QIcon
(
":/images/mavs/fixed-wing.svg"
);
break
;
case
2
:
case
MAV_TYPE_QUADROTOR
:
icon
=
QIcon
(
":/images/mavs/quadrotor.svg"
);
break
;
case
3
:
case
MAV_TYPE_COAXIAL
:
icon
=
QIcon
(
":/images/mavs/coaxial.svg"
);
break
;
case
4
:
case
MAV_TYPE_HELICOPTER
:
icon
=
QIcon
(
":/images/mavs/helicopter.svg"
);
break
;
case
5
:
case
MAV_TYPE_GCS
:
icon
=
QIcon
(
":/images/mavs/groundstation.svg"
);
break
;
default:
...
...
src/ui/SerialConfigurationWindow.cc
View file @
8547fa05
...
...
@@ -59,39 +59,55 @@ SerialConfigurationWindow::SerialConfigurationWindow(LinkInterface* link, QWidge
// Set up baud rates
ui
.
baudRate
->
clear
();
ui
.
baudRate
->
addItem
(
"50"
,
50
);
ui
.
baudRate
->
addItem
(
"70"
,
70
);
ui
.
baudRate
->
addItem
(
"110"
,
110
);
ui
.
baudRate
->
addItem
(
"134"
,
134
);
ui
.
baudRate
->
addItem
(
"150"
,
150
);
ui
.
baudRate
->
addItem
(
"200"
,
200
);
ui
.
baudRate
->
addItem
(
"300"
,
300
);
ui
.
baudRate
->
addItem
(
"600"
,
600
);
ui
.
baudRate
->
addItem
(
"1200"
,
1200
);
ui
.
baudRate
->
addItem
(
"1800"
,
1800
);
ui
.
baudRate
->
addItem
(
"2400"
,
2400
);
ui
.
baudRate
->
addItem
(
"4800"
,
4800
);
ui
.
baudRate
->
addItem
(
"9600"
,
9600
);
#ifdef Q_OS_WIN
ui
.
baudRate
->
addItem
(
"14400"
,
14400
);
#endif
ui
.
baudRate
->
addItem
(
"19200"
,
19200
);
ui
.
baudRate
->
addItem
(
"38400"
,
38400
);
#ifdef Q_OS_WIN
ui
.
baudRate
->
addItem
(
"56000"
,
56000
);
// Keep track of all desired baud rates by OS. These are iterated through
// later and added to ui.baudRate.
QList
<
int
>
supportedBaudRates
;
// Baud rates supported only by POSIX systems
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
supportedBaudRates
<<
50
;
supportedBaudRates
<<
75
;
supportedBaudRates
<<
134
;
supportedBaudRates
<<
150
;
supportedBaudRates
<<
200
;
supportedBaudRates
<<
1800
;
#endif
ui
.
baudRate
->
addItem
(
"57600"
,
57600
);
#ifdef Q_OS_WIN
ui
.
baudRate
->
addItem
(
"76800"
,
76800
);
// Baud rates supported only by Linux
#if defined(Q_OS_LINUX)
supportedBaudRates
<<
230400
;
supportedBaudRates
<<
460800
;
supportedBaudRates
<<
500000
;
supportedBaudRates
<<
576000
;
supportedBaudRates
<<
921600
;
#endif
ui
.
baudRate
->
addItem
(
"115200"
,
115200
);
#ifdef Q_OS_WIN
ui
.
baudRate
->
addItem
(
"128000"
,
128000
);
ui
.
baudRate
->
addItem
(
"230400"
,
230400
);
ui
.
baudRate
->
addItem
(
"256000"
,
256000
);
ui
.
baudRate
->
addItem
(
"460800"
,
460800
);
// Baud rates supported only by Windows
#if defined(Q_OS_WIN)
supportedBaudRates
<<
14400
;
supportedBaudRates
<<
56000
;
supportedBaudRates
<<
128000
;
supportedBaudRates
<<
256000
;
#endif
ui
.
baudRate
->
addItem
(
"921600"
,
921600
);
// Baud rates supported by everyone
supportedBaudRates
<<
110
;
supportedBaudRates
<<
300
;
supportedBaudRates
<<
600
;
supportedBaudRates
<<
1200
;
supportedBaudRates
<<
2400
;
supportedBaudRates
<<
4800
;
supportedBaudRates
<<
9600
;
supportedBaudRates
<<
19200
;
supportedBaudRates
<<
38400
;
supportedBaudRates
<<
57600
;
supportedBaudRates
<<
115200
;
// Now actually add all of our supported baud rates to the UI.
qSort
(
supportedBaudRates
.
begin
(),
supportedBaudRates
.
end
());
for
(
int
i
=
0
;
i
<
supportedBaudRates
.
size
();
++
i
)
{
ui
.
baudRate
->
addItem
(
QString
::
number
(
supportedBaudRates
.
at
(
i
)),
supportedBaudRates
.
at
(
i
));
}
connect
(
action
,
SIGNAL
(
triggered
()),
this
,
SLOT
(
configureCommunication
()));
...
...
src/ui/mavlink/QGCMAVLinkMessageSender.cc
View file @
8547fa05
...
...
@@ -39,7 +39,7 @@ void QGCMAVLinkMessageSender::refresh()
bool
QGCMAVLinkMessageSender
::
sendMessage
()
{
sendMessage
(
ui
->
messageIdSpinBox
->
value
());
return
sendMessage
(
ui
->
messageIdSpinBox
->
value
());
}
bool
QGCMAVLinkMessageSender
::
sendMessage
(
unsigned
int
msgid
)
...
...
thirdParty/qserialport/include/QtSerialPort/qportsettings.h
View file @
8547fa05
...
...
@@ -51,30 +51,30 @@ enum ChangeApplyTypes { AllAppTy, PortAttrOnlyAppTy, CommTimeoutsOnlyAppTy };
*/
struct
CommTimeouts
{
#ifdef TNX_WINDOWS_SERIAL_PORT
typedef
DWORD
commt_t
;
static
const
DWORD
NoTimeout
=
MAXDWORD
;
typedef
DWORD
commt_t
;
static
const
DWORD
NoTimeout
=
MAXDWORD
;
#else
typedef
quint8
commt_t
;
static
const
qint8
NoTimeout
=
-
1
;
typedef
quint8
commt_t
;
static
const
qint8
NoTimeout
=
-
1
;
#endif
// Win32 only section
commt_t
Win32ReadIntervalTimeout
;
///< Maximum time between read chars. Win32 only.
commt_t
Win32ReadTotalTimeoutMultiplier
;
///< Multiplier of characters. Win32 only.
commt_t
Win32ReadTotalTimeoutConstant
;
///< Constant in milliseconds. Win32 only.
commt_t
Win32WriteTotalTimeoutMultiplier
;
///< Multiplier of characters. Win32 only.
commt_t
Win32WriteTotalTimeoutConstant
;
///< Constant in milliseconds. Win32 only.
// Win32 only section
commt_t
Win32ReadIntervalTimeout
;
///< Maximum time between read chars. Win32 only.
commt_t
Win32ReadTotalTimeoutMultiplier
;
///< Multiplier of characters. Win32 only.
commt_t
Win32ReadTotalTimeoutConstant
;
///< Constant in milliseconds. Win32 only.
commt_t
Win32WriteTotalTimeoutMultiplier
;
///< Multiplier of characters. Win32 only.
commt_t
Win32WriteTotalTimeoutConstant
;
///< Constant in milliseconds. Win32 only.
// Posix only section
commt_t
PosixVTIME
;
///< Read timeout. Posix only.
commt_t
PosixVMIN
;
///< Minimum number of bytes before returning from
///< read operation. Posix only.
CommTimeouts
()
:
Win32ReadIntervalTimeout
(
NoTimeout
),
Win32ReadTotalTimeoutMultiplier
(
0
),
Win32ReadTotalTimeoutConstant
(
0
),
Win32WriteTotalTimeoutMultiplier
(
25
),
Win32WriteTotalTimeoutConstant
(
250
),
PosixVTIME
(
0
),
PosixVMIN
(
1
)
{
}
// Posix only section
commt_t
PosixVTIME
;
///< Read timeout. Posix only.
commt_t
PosixVMIN
;
///< Minimum number of bytes before returning from
///< read operation. Posix only.
CommTimeouts
()
:
Win32ReadIntervalTimeout
(
NoTimeout
),
Win32ReadTotalTimeoutMultiplier
(
0
),
Win32ReadTotalTimeoutConstant
(
0
),
Win32WriteTotalTimeoutMultiplier
(
25
),
Win32WriteTotalTimeoutConstant
(
250
),
PosixVTIME
(
0
),
PosixVMIN
(
1
)
{
}
};
/**
...
...
@@ -83,175 +83,179 @@ struct CommTimeouts {
class
TONIX_EXPORT
QPortSettings
{
public:
enum
BaudRate
{
BAUDR_UNKNOWN
=
0
,
enum
BaudRate
{
BAUDR_UNKNOWN
=
0
,
#ifdef TNX_POSIX_SERIAL_PORT
BAUDR_50
,
BAUDR_75
,
BAUDR_134
,
BAUDR_150
,
BAUDR_200
,
BAUDR_1800
,
//BAUDR_76800,
BAUDR_500000
,
BAUDR_576000
,
BAUDR_50
,
BAUDR_75
,
BAUDR_134
,
BAUDR_150
,
BAUDR_200
,
BAUDR_1800
,
//BAUDR_76800,
#endif
#ifdef Q_OS_LINUX
// BAUDR_500000,
// BAUDR_576000,
#endif
#ifdef TNX_WINDOWS_SERIAL_PORT
BAUDR_14400
,
BAUDR_56000
,
BAUDR_128000
,
BAUDR_256000
,
BAUDR_14400
,
BAUDR_56000
,
BAUDR_128000
,
BAUDR_256000
,
#endif
// baud rates supported by all OSs
BAUDR_110
,
BAUDR_300
,
BAUDR_600
,
BAUDR_1200
,
BAUDR_2400
,
BAUDR_4800
,
BAUDR_9600
,
BAUDR_19200
,
BAUDR_38400
,
BAUDR_57600
,
BAUDR_115200
,
BAUDR_230400
,
BAUDR_460800
,
BAUDR_921600
};
// baud rates supported by all OSs
BAUDR_110
,
BAUDR_300
,
BAUDR_600
,
BAUDR_1200
,
BAUDR_2400
,
BAUDR_4800
,
BAUDR_9600
,
BAUDR_19200
,
BAUDR_38400
,
BAUDR_57600
,
BAUDR_115200
,
BAUDR_230400
,
BAUDR_460800
,
BAUDR_500000
,
BAUDR_576000
,
BAUDR_921600
,
};
enum
DataBits
{
DB_5
,
// simulated in POSIX
DB_6
,
DB_7
,
DB_8
,
DB_UNKNOWN
};
enum
DataBits
{
DB_5
,
// simulated in POSIX
DB_6
,
DB_7
,
DB_8
,
DB_UNKNOWN
};
enum
Parity
{
PAR_NONE
,
PAR_ODD
,
PAR_EVEN
,
enum
Parity
{
PAR_NONE
,
PAR_ODD
,
PAR_EVEN
,
#ifdef TNX_WINDOWS_SERIAL_PORT
PAR_MARK
,
PAR_MARK
,
#endif
PAR_SPACE
,
// simulated in POSIX
PAR_UNKNOWN
};
PAR_SPACE
,
// simulated in POSIX
PAR_UNKNOWN
};
enum
StopBits
{
STOP_1
,
enum
StopBits
{
STOP_1
,
#ifdef TNX_WINDOWS_SERIAL_PORT
STOP_1_5
,
STOP_1_5
,
#endif
STOP_2
,
STOP_UNKNOWN
};
STOP_2
,
STOP_UNKNOWN
};
enum
FlowControl
{
FLOW_OFF
,
FLOW_HARDWARE
,
FLOW_XONXOFF
,
FLOW_UNKNOWN
};
enum
FlowControl
{
FLOW_OFF
,
FLOW_HARDWARE
,
FLOW_XONXOFF
,
FLOW_UNKNOWN
};
QPortSettings
();
QPortSettings
(
const
QString
&
settings
);
QPortSettings
();
QPortSettings
(
const
QString
&
settings
);
// port configuration methods
// port configuration methods
bool
set
(
const
QString
&
settings
);
bool
set
(
const
QString
&
settings
);
inline
BaudRate
baudRate
()
const
{
return
baudRate_
;
}
void
setBaudRate
(
BaudRate
baudRate
)
{
baudRate_
=
baudRate
;
inline
BaudRate
baudRate
()
const
{
return
baudRate_
;
}
void
setBaudRate
(
BaudRate
baudRate
)
{
baudRate_
=
baudRate
;
switch
(
baudRate_
)
{
#ifdef TNX_POSIX_SERIAL_PORT
case
BAUDR_50
:
baudRateInt_
=
50
;
break
;
case
BAUDR_75
:
baudRateInt_
=
75
;
break
;
case
BAUDR_134
:
baudRateInt_
=
134
;
break
;
case
BAUDR_150
:
baudRateInt_
=
150
;
break
;
case
BAUDR_200
:
baudRateInt_
=
200
;
break
;
case
BAUDR_1800
:
baudRateInt_
=
1800
;
break
;
//case 76800: baudRateInt_=76800; break;
#endif
#ifdef TNX_WINDOWS_SERIAL_PORT
case
BAUDR_14400
:
baudRateInt_
=
14400
;
break
;
case
BAUDR_56000
:
baudRateInt_
=
56000
;
break
;
case
BAUDR_128000
:
baudRateInt_
=
128000
;
break
;
case
BAUDR_256000
:
baudRateInt_
=
256000
;
break
;
#endif
#if defined(Q_OS_LINUX)
case
BAUDR_500000
:
baudRateInt_
=
500000
;
break
;
case
BAUDR_576000
:
baudRateInt_
=
576000
;
break
;
#endif
// baud rates supported by all platforms
case
BAUDR_110
:
baudRateInt_
=
110
;
break
;
case
BAUDR_300
:
baudRateInt_
=
300
;
break
;
case
BAUDR_600
:
baudRateInt_
=
600
;
break
;
case
BAUDR_1200
:
baudRateInt_
=
1200
;
break
;
case
BAUDR_2400
:
baudRateInt_
=
2400
;
break
;
case
BAUDR_4800
:
baudRateInt_
=
4800
;
break
;
case
BAUDR_9600
:
baudRateInt_
=
9600
;
break
;
case
BAUDR_19200
:
baudRateInt_
=
19200
;
break
;
case
BAUDR_38400
:
baudRateInt_
=
38400
;
break
;
case
BAUDR_57600
:
baudRateInt_
=
57600
;
break
;
case
BAUDR_115200
:
baudRateInt_
=
115200
;
break
;
case
BAUDR_230400
:
baudRateInt_
=
230400
;
break
;
case
BAUDR_460800
:
baudRateInt_
=
460800
;
break
;
case
BAUDR_921600
:
baudRateInt_
=
921600
;
break
;
default:
baudRateInt_
=
0
;
// unknown baudrate
}
}
switch
(
baudRate_
)
{
#ifdef TNX_POSIX_SERIAL_PORT
case
BAUDR_50
:
baudRateInt_
=
50
;
break
;
case
BAUDR_75
:
baudRateInt_
=
75
;
break
;
case
BAUDR_134
:
baudRateInt_
=
134
;
break
;
case
BAUDR_150
:
baudRateInt_
=
150
;
break
;
case
BAUDR_200
:
baudRateInt_
=
200
;
break
;
case
BAUDR_1800
:
baudRateInt_
=
1800
;
break
;
//case 76800: baudRateInt_=76800; break;
#endif
#ifdef TNX_WINDOWS_SERIAL_PORT
case
BAUDR_14400
:
baudRateInt_
=
14400
;
break
;
case
BAUDR_56000
:
baudRateInt_
=
56000
;
break
;
case
BAUDR_128000
:
baudRateInt_
=
128000
;
break
;
case
BAUDR_256000
:
baudRateInt_
=
256000
;
break
;
#endif
#if defined(Q_OS_LINUX)
case
BAUDR_500000
:
baudRateInt_
=
500000
;
break
;
case
BAUDR_576000
:
baudRateInt_
=
576000
;
break
;
#endif
// baud rates supported by all platforms
case
BAUDR_110
:
baudRateInt_
=
110
;
break
;
case
BAUDR_300
:
baudRateInt_
=
300
;
break
;
case
BAUDR_600
:
baudRateInt_
=
600
;
break
;
case
BAUDR_1200
:
baudRateInt_
=
1200
;
break
;
case
BAUDR_2400
:
baudRateInt_
=
2400
;
break
;
case
BAUDR_4800
:
baudRateInt_
=
4800
;
break
;
case
BAUDR_9600
:
baudRateInt_
=
9600
;
break
;
case
BAUDR_19200
:
baudRateInt_
=
19200
;
break
;
case
BAUDR_38400
:
baudRateInt_
=
38400
;
break
;
case
BAUDR_57600
:
baudRateInt_
=
57600
;
break
;
case
BAUDR_115200
:
baudRateInt_
=
115200
;
break
;
case
BAUDR_230400
:
baudRateInt_
=
230400
;
break
;
case
BAUDR_460800
:
baudRateInt_
=
460800
;
break
;
case
BAUDR_921600
:
baudRateInt_
=
921600
;
break
;
default:
baudRateInt_
=
0
;
// unknown baudrate
}
}
inline
Parity
parity
()
const
{
return
parity_
;
}
inline
void
setParity
(
Parity
parity
)
{
parity_
=
parity
;
}
inline
Parity
parity
()
const
{
return
parity_
;
}
inline
void
setParity
(
Parity
parity
)
{
parity_
=
parity
;
}
inline
StopBits
stopBits
()
const
{
return
stopBits_
;
}
inline
void
setStopBits
(
StopBits
stopBits
)
{
stopBits_
=
stopBits
;
}
inline
StopBits
stopBits
()
const
{
return
stopBits_
;
}
inline
void
setStopBits
(
StopBits
stopBits
)
{
stopBits_
=
stopBits
;
}
inline
DataBits
dataBits
()
const
{
return
dataBits_
;
}
inline
void
setDataBits
(
DataBits
dataBits
)
{
dataBits_
=
dataBits
;
}
inline
DataBits
dataBits
()
const
{
return
dataBits_
;
}
inline
void
setDataBits
(
DataBits
dataBits
)
{
dataBits_
=
dataBits
;
}
inline
FlowControl
flowControl
()
const
{
return
flowControl_
;
}
inline
void
setFlowControl
(
FlowControl
flowControl
)
{
flowControl_
=
flowControl
;
}
inline
FlowControl
flowControl
()
const
{
return
flowControl_
;
}
inline
void
setFlowControl
(
FlowControl
flowControl
)
{
flowControl_
=
flowControl
;
}
QString
toString
()
const
;
QString
toString
()
const
;
// helper methods to configure port settings
// helper methods to configure port settings
private:
static
BaudRate
baudRateFromInt
(
int
baud
,
bool
&
ok
);
static
DataBits
dataBitsFromString
(
const
QString
&
databits
,
bool
&
ok
);
static
Parity
parityFromString
(
const
QString
&
parity
,
bool
&
ok
);
static
StopBits
stopBitsFromString
(
const
QString
&
stopbits
,
bool
&
ok
);
static
FlowControl
flowControlFromString
(
const
QString
&
flow
,
bool
&
ok
);
static
BaudRate
baudRateFromInt
(
int
baud
,
bool
&
ok
);
static
DataBits
dataBitsFromString
(
const
QString
&
databits
,
bool
&
ok
);
static
Parity
parityFromString
(
const
QString
&
parity
,
bool
&
ok
);
static
StopBits
stopBitsFromString
(
const
QString
&
stopbits
,
bool
&
ok
);
static
FlowControl
flowControlFromString
(
const
QString
&
flow
,
bool
&
ok
);
private:
BaudRate
baudRate_
;
DataBits
dataBits_
;
Parity
parity_
;
StopBits
stopBits_
;
FlowControl
flowControl_
;
qint32
baudRateInt_
;
BaudRate
baudRate_
;
DataBits
dataBits_
;
Parity
parity_
;
StopBits
stopBits_
;
FlowControl
flowControl_
;
qint32
baudRateInt_
;
};
}
...
...
thirdParty/qserialport/src/common/qportsettings.cpp
View file @
8547fa05
...
...
@@ -31,11 +31,11 @@ namespace TNX {
QPortSettings
::
QPortSettings
()
{
setBaudRate
(
BAUDR_9600
);
dataBits_
=
DB_8
;
parity_
=
PAR_NONE
;
stopBits_
=
STOP_1
;
flowControl_
=
FLOW_OFF
;
setBaudRate
(
BAUDR_9600
);
dataBits_
=
DB_8
;
parity_
=
PAR_NONE
;
stopBits_
=
STOP_1
;
flowControl_
=
FLOW_OFF
;
}
/*!
...
...
@@ -44,72 +44,72 @@ QPortSettings::QPortSettings()
QPortSettings
::
QPortSettings
(
const
QString
&
settings
)
{
set
(
settings
);
set
(
settings
);
}
bool
QPortSettings
::
set
(
const
QString
&
settings
)
{
setBaudRate
(
BAUDR_9600
);
dataBits_
=
DB_8
;
parity_
=
PAR_NONE
;
stopBits_
=
STOP_1
;
flowControl_
=
FLOW_OFF
;
setBaudRate
(
BAUDR_9600
);
dataBits_
=
DB_8
;
parity_
=
PAR_NONE
;
stopBits_
=
STOP_1
;
flowControl_
=
FLOW_OFF
;
QStringList
list
=
settings
.
split
(
","
,
QString
::
SkipEmptyParts
);
bool
ok
=
true
;
for
(
int
i
=
0
;
i
<
list
.
size
();
i
++
)
{
switch
(
i
)
{
case
0
:
setBaudRate
(
baudRateFromInt
(
list
.
at
(
i
).
toInt
(
&
ok
),
ok
));
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting baud rate [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
1
:
dataBits_
=
dataBitsFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting data bits [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
2
:
parity_
=
parityFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting parity [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
3
:
stopBits_
=
stopBitsFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting stop bits [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
4
:
flowControl_
=
flowControlFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting flow control type [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
default:
break
;
}
//switch
}
//for
QStringList
list
=
settings
.
split
(
","
,
QString
::
SkipEmptyParts
);
bool
ok
=
true
;
for
(
int
i
=
0
;
i
<
list
.
size
();
i
++
)
{
switch
(
i
)
{
case
0
:
setBaudRate
(
baudRateFromInt
(
list
.
at
(
i
).
toInt
(
&
ok
),
ok
));
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting baud rate [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
1
:
dataBits_
=
dataBitsFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting data bits [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
2
:
parity_
=
parityFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting parity [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
3
:
stopBits_
=
stopBitsFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting stop bits [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
case
4
:
flowControl_
=
flowControlFromString
(
list
.
at
(
i
),
ok
);
if
(
!
ok
)
{
qWarning
()
<<
QString
(
"QPortSettings::QPortSettings(%1) "
\
"failed when setting flow control type [value: %2]; using default value"
)
.
arg
(
settings
)
.
arg
(
list
.
at
(
i
));
}
break
;
default:
break
;
}
//switch
}
//for
return
ok
;
return
ok
;
}
/*!
...
...
@@ -117,74 +117,74 @@ bool QPortSettings::set(const QString &settings)
*/
QPortSettings
::
BaudRate
QPortSettings
::
baudRateFromInt
(
int
baud
,
bool
&
ok
)
{
ok
=
true
;
ok
=
true
;
switch
(
baud
)
{
switch
(
baud
)
{
#ifdef TNX_POSIX_SERIAL_PORT
case
50
:
return
BAUDR_50
;
return
BAUDR_50
;
case
75
:
return
BAUDR_75
;
return
BAUDR_75
;
case
134
:
return
BAUDR_134
;
return
BAUDR_134
;
case
150
:
return
BAUDR_150
;
return
BAUDR_150
;
case
200
:
return
BAUDR_200
;
return
BAUDR_200
;
case
1800
:
return
BAUDR_1800
;
//case 76800:
// return BAUDR_76800;
return
BAUDR_1800
;
//case 76800:
// return BAUDR_76800;
#endif
#ifdef TNX_WINDOWS_SERIAL_PORT
case
14400
:
return
BAUDR_14400
;
return
BAUDR_14400
;
case
56000
:
return
BAUDR_56000
;
return
BAUDR_56000
;
case
128000
:
return
BAUDR_128000
;
return
BAUDR_128000
;
case
256000
:
return
BAUDR_256000
;
return
BAUDR_256000
;
#endif
#if defined(Q_OS_LINUX)
case
230400
:
return
BAUDR_230400
;
case
460800
:
return
BAUDR_460800
;
case
500000
:
return
BAUDR_500000
;
return
BAUDR_500000
;
case
576000
:
return
BAUDR_576000
;
case
921600
:
return
BAUDR_921600
;
return
BAUDR_576000
;
#endif
// baud rates supported by all platforms
// baud rates supported by all platforms
case
110
:
return
BAUDR_110
;
return
BAUDR_110
;
case
300
:
return
BAUDR_300
;
return
BAUDR_300
;
case
600
:
return
BAUDR_600
;
return
BAUDR_600
;
case
1200
:
return
BAUDR_1200
;
return
BAUDR_1200
;
case
2400
:
return
BAUDR_2400
;
return
BAUDR_2400
;
case
4800
:
return
BAUDR_4800
;
return
BAUDR_4800
;
case
9600
:
return
BAUDR_9600
;
return
BAUDR_9600
;
case
19200
:
return
BAUDR_19200
;
return
BAUDR_19200
;
case
38400
:
return
BAUDR_38400
;
return
BAUDR_38400
;
case
57600
:
return
BAUDR_57600
;
return
BAUDR_57600
;
case
115200
:
return
BAUDR_115200
;
return
BAUDR_115200
;
case
230400
:
return
BAUDR_230400
;
case
460800
:
return
BAUDR_460800
;
case
921600
:
return
BAUDR_921600
;
default:
ok
=
false
;
return
BAUDR_9600
;
}
ok
=
false
;
return
BAUDR_9600
;
}
}
/*!
...
...
@@ -192,19 +192,19 @@ QPortSettings::BaudRate QPortSettings::baudRateFromInt(int baud, bool &ok)
*/
QPortSettings
::
DataBits
QPortSettings
::
dataBitsFromString
(
const
QString
&
dataBits
,
bool
&
ok
)
{
ok
=
true
;
if
(
dataBits
.
trimmed
()
==
"5"
)
return
DB_5
;
else
if
(
dataBits
.
trimmed
()
==
"6"
)
return
DB_6
;
else
if
(
dataBits
.
trimmed
()
==
"7"
)
return
DB_7
;
else
if
(
dataBits
.
trimmed
()
==
"8"
)
return
DB_8
;
else
{
ok
=
false
;
return
DB_8
;
}
ok
=
true
;
if
(
dataBits
.
trimmed
()
==
"5"
)
return
DB_5
;
else
if
(
dataBits
.
trimmed
()
==
"6"
)
return
DB_6
;
else
if
(
dataBits
.
trimmed
()
==
"7"
)
return
DB_7
;
else
if
(
dataBits
.
trimmed
()
==
"8"
)
return
DB_8
;
else
{
ok
=
false
;
return
DB_8
;
}
}
/*!
...
...
@@ -212,23 +212,23 @@ QPortSettings::DataBits QPortSettings::dataBitsFromString(const QString &dataBit
*/
QPortSettings
::
Parity
QPortSettings
::
parityFromString
(
const
QString
&
parity
,
bool
&
ok
)
{
ok
=
true
;
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"n"
,
Qt
::
CaseInsensitive
)
)
return
PAR_NONE
;
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"o"
,
Qt
::
CaseInsensitive
)
)
return
PAR_ODD
;
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"e"
,
Qt
::
CaseInsensitive
)
)
return
PAR_EVEN
;
ok
=
true
;
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"n"
,
Qt
::
CaseInsensitive
)
)
return
PAR_NONE
;
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"o"
,
Qt
::
CaseInsensitive
)
)
return
PAR_ODD
;
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"e"
,
Qt
::
CaseInsensitive
)
)
return
PAR_EVEN
;
#ifdef TNX_WINDOWS_SERIAL_PORT
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"m"
,
Qt
::
CaseInsensitive
)
)
return
PAR_MARK
;
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"m"
,
Qt
::
CaseInsensitive
)
)
return
PAR_MARK
;
#endif
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"s"
,
Qt
::
CaseInsensitive
)
)
return
PAR_SPACE
;
else
{
ok
=
false
;
return
PAR_NONE
;
}
else
if
(
!
QString
::
compare
(
parity
.
trimmed
(),
"s"
,
Qt
::
CaseInsensitive
)
)
return
PAR_SPACE
;
else
{
ok
=
false
;
return
PAR_NONE
;
}
}
/*!
...
...
@@ -236,19 +236,19 @@ QPortSettings::Parity QPortSettings::parityFromString(const QString &parity, boo
*/
QPortSettings
::
StopBits
QPortSettings
::
stopBitsFromString
(
const
QString
&
stopBits
,
bool
&
ok
)
{
ok
=
true
;
if
(
stopBits
.
trimmed
()
==
"1"
)
return
STOP_1
;
ok
=
true
;
if
(
stopBits
.
trimmed
()
==
"1"
)
return
STOP_1
;
#ifdef TNX_WINDOWS_SERIAL_PORT
else
if
(
stopBits
.
trimmed
()
==
"1.5"
)
return
STOP_1_5
;
else
if
(
stopBits
.
trimmed
()
==
"1.5"
)
return
STOP_1_5
;
#endif
else
if
(
stopBits
.
trimmed
()
==
"2"
)
return
STOP_2
;
else
{
ok
=
false
;
return
STOP_1
;
}
else
if
(
stopBits
.
trimmed
()
==
"2"
)
return
STOP_2
;
else
{
ok
=
false
;
return
STOP_1
;
}
}
/*!
...
...
@@ -256,17 +256,17 @@ QPortSettings::StopBits QPortSettings::stopBitsFromString(const QString &stopBit
*/
QPortSettings
::
FlowControl
QPortSettings
::
flowControlFromString
(
const
QString
&
flow
,
bool
&
ok
)
{
ok
=
true
;
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"off"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_OFF
;
else
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"xon/xoff"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_XONXOFF
;
else
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"hardware"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_HARDWARE
;
else
{
ok
=
false
;
return
FLOW_OFF
;
}
ok
=
true
;
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"off"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_OFF
;
else
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"xon/xoff"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_XONXOFF
;
else
if
(
!
QString
::
compare
(
flow
.
trimmed
(),
"hardware"
,
Qt
::
CaseInsensitive
)
)
return
FLOW_HARDWARE
;
else
{
ok
=
false
;
return
FLOW_OFF
;
}
}
/*!
...
...
@@ -274,50 +274,50 @@ QPortSettings::FlowControl QPortSettings::flowControlFromString(const QString &f
*/
QString
QPortSettings
::
toString
()
const
{
QString
txt
;
QString
txt
;
txt
.
setNum
(
baudRateInt_
,
10
);
txt
.
append
(
","
);
txt
.
setNum
(
baudRateInt_
,
10
);
txt
.
append
(
","
);
if
(
dataBits
()
==
DB_5
)
txt
.
append
(
"5,"
);
else
if
(
dataBits
()
==
DB_6
)
txt
.
append
(
"6,"
);
else
if
(
dataBits
()
==
DB_7
)
txt
.
append
(
"7,"
);
else
if
(
dataBits
()
==
DB_8
)
txt
.
append
(
"8,"
);
if
(
dataBits
()
==
DB_5
)
txt
.
append
(
"5,"
);
else
if
(
dataBits
()
==
DB_6
)
txt
.
append
(
"6,"
);
else
if
(
dataBits
()
==
DB_7
)
txt
.
append
(
"7,"
);
else
if
(
dataBits
()
==
DB_8
)
txt
.
append
(
"8,"
);
if
(
parity
()
==
PAR_NONE
)
txt
.
append
(
"N,"
);
else
if
(
parity
()
==
PAR_ODD
)
txt
.
append
(
"O,"
);
else
if
(
parity
()
==
PAR_EVEN
)
txt
.
append
(
"E,"
);
if
(
parity
()
==
PAR_NONE
)
txt
.
append
(
"N,"
);
else
if
(
parity
()
==
PAR_ODD
)
txt
.
append
(
"O,"
);
else
if
(
parity
()
==
PAR_EVEN
)
txt
.
append
(
"E,"
);
#ifdef TNX_WINDOWS_SERIAL_PORT
else
if
(
parity
()
==
PAR_MARK
)
txt
.
append
(
"M,"
);
else
if
(
parity
()
==
PAR_MARK
)
txt
.
append
(
"M,"
);
#endif
else
if
(
parity
()
==
PAR_SPACE
)
txt
.
append
(
"S,"
);
else
if
(
parity
()
==
PAR_SPACE
)
txt
.
append
(
"S,"
);
if
(
stopBits
()
==
STOP_1
)
txt
.
append
(
"1,"
);
if
(
stopBits
()
==
STOP_1
)
txt
.
append
(
"1,"
);
#ifdef TNX_WINDOWS_SERIAL_PORT
else
if
(
stopBits
()
==
STOP_1_5
)
txt
.
append
(
"1.5,"
);
else
if
(
stopBits
()
==
STOP_1_5
)
txt
.
append
(
"1.5,"
);
#endif
else
if
(
stopBits
()
==
STOP_2
)
txt
.
append
(
"2,"
);
else
if
(
stopBits
()
==
STOP_2
)
txt
.
append
(
"2,"
);
if
(
flowControl
()
==
FLOW_OFF
)
txt
.
append
(
"off"
);
else
if
(
flowControl
()
==
FLOW_XONXOFF
)
txt
.
append
(
"xon/xoff"
);
else
if
(
flowControl
()
==
FLOW_HARDWARE
)
txt
.
append
(
"hardware"
);
if
(
flowControl
()
==
FLOW_OFF
)
txt
.
append
(
"off"
);
else
if
(
flowControl
()
==
FLOW_XONXOFF
)
txt
.
append
(
"xon/xoff"
);
else
if
(
flowControl
()
==
FLOW_HARDWARE
)
txt
.
append
(
"hardware"
);
return
txt
;
return
txt
;
}
}
// namespace
...
...
thirdParty/qserialport/src/posix/termioshelper.cpp
View file @
8547fa05
...
...
@@ -33,39 +33,39 @@ namespace TNX {
*/
TermiosHelper
::
TermiosHelper
(
int
fileDescriptor
)
:
fileDescriptor_
(
fileDescriptor
),
originalAttrs_
(
NULL
),
currentAttrs_
(
NULL
)
:
fileDescriptor_
(
fileDescriptor
),
originalAttrs_
(
NULL
),
currentAttrs_
(
NULL
)
{
Q_ASSERT
(
fileDescriptor_
>
0
);
Q_ASSERT
(
fileDescriptor_
>
0
);
originalAttrs_
=
new
termios
();
currentAttrs_
=
new
termios
();
originalAttrs_
=
new
termios
();
currentAttrs_
=
new
termios
();
// save the current serial port attributes
// see restoreTermios()
// save the current serial port attributes
// see restoreTermios()
saveTermios
();
saveTermios
();
// clone the original attributes
// clone the original attributes
*
currentAttrs_
=
*
originalAttrs_
;
*
currentAttrs_
=
*
originalAttrs_
;
// initialize port attributes for serial port communication
// initialize port attributes for serial port communication
initTermios
();
initTermios
();
}
TermiosHelper
::~
TermiosHelper
()
{
// It is good practice to reset a serial port back to the state in
// which you found it. This is why we saved the original termios struct
// The constant TCSANOW (defined in termios.h) indicates that
// the change should take effect immediately.
// It is good practice to reset a serial port back to the state in
// which you found it. This is why we saved the original termios struct
// The constant TCSANOW (defined in termios.h) indicates that
// the change should take effect immediately.
restoreTermios
();
restoreTermios
();
delete
originalAttrs_
;
delete
currentAttrs_
;
delete
originalAttrs_
;
delete
currentAttrs_
;
}
/*!
...
...
@@ -74,18 +74,18 @@ TermiosHelper::~TermiosHelper()
bool
TermiosHelper
::
applyChanges
(
ChangeApplyTypes
/*apptype*/
)
{
// there is only termios structure to be applied for posix compatible platforms
if
(
tcsetattr
(
fileDescriptor_
,
TCSANOW
,
currentAttrs_
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::applyChanges(file: %1, applyType: %2) failed "
\
"when setting new attributes: %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
TCSANOW
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
return
true
;
// there is only termios structure to be applied for posix compatible platforms
if
(
tcsetattr
(
fileDescriptor_
,
TCSANOW
,
currentAttrs_
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::applyChanges(file: %1, applyType: %2) failed "
\
"when setting new attributes: %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
TCSANOW
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
return
true
;
}
/*!
...
...
@@ -94,33 +94,33 @@ bool TermiosHelper::applyChanges(ChangeApplyTypes /*apptype*/)
bool
TermiosHelper
::
setCtrSignal
(
ControlSignals
csig
,
bool
value
)
{
int
status
;
if
(
ioctl
(
fileDescriptor_
,
TIOCMGET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setCtrSignal(file: %1, csig: %2) failed"
\
"when fetching control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
if
(
value
)
status
|=
(
int
)
csig
;
else
status
&=
~
((
int
)
csig
);
if
(
ioctl
(
fileDescriptor_
,
TIOCMSET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setCtrSignal(file: %1, csig: %2) failed"
\
"when setting control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
return
true
;
int
status
;
if
(
ioctl
(
fileDescriptor_
,
TIOCMGET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setCtrSignal(file: %1, csig: %2) failed"
\
"when fetching control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
if
(
value
)
status
|=
(
int
)
csig
;
else
status
&=
~
((
int
)
csig
);
if
(
ioctl
(
fileDescriptor_
,
TIOCMSET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setCtrSignal(file: %1, csig: %2) failed"
\
"when setting control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
return
true
;
}
/*!
...
...
@@ -129,19 +129,19 @@ bool TermiosHelper::setCtrSignal(ControlSignals csig, bool value)
QSerialPort
::
CommSignalValues
TermiosHelper
::
ctrSignal
(
ControlSignals
csig
)
const
{
int
status
;
if
(
ioctl
(
fileDescriptor_
,
TIOCMGET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::ctrSignal(file: %1, csig: %2) failed"
\
"when fetching control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QSerialPort
::
Signal_Unknown
;
}
return
(
status
&
((
int
)
csig
))
?
QSerialPort
::
Signal_On
:
QSerialPort
::
Signal_Off
;
int
status
;
if
(
ioctl
(
fileDescriptor_
,
TIOCMGET
,
&
status
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::ctrSignal(file: %1, csig: %2) failed"
\
"when fetching control signal values : %3(%4)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
csig
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QSerialPort
::
Signal_Unknown
;
}
return
(
status
&
((
int
)
csig
))
?
QSerialPort
::
Signal_On
:
QSerialPort
::
Signal_Off
;
}
/*!
...
...
@@ -150,24 +150,24 @@ QSerialPort::CommSignalValues TermiosHelper::ctrSignal(ControlSignals csig) cons
void
TermiosHelper
::
initTermios
()
{
// Set raw input (non-canonical) mode, with reads blocking until either a single character
// has been received or a one second timeout expires.
// See tcsetattr(4) ("man 4 tcsetattr") and termios(4) ("man 4 termios") for details.
// Set raw input (non-canonical) mode, with reads blocking until either a single character
// has been received or a one second timeout expires.
// See tcsetattr(4) ("man 4 tcsetattr") and termios(4) ("man 4 termios") for details.
cfmakeraw
(
currentAttrs_
);
cfmakeraw
(
currentAttrs_
);
currentAttrs_
->
c_cflag
|=
(
CLOCAL
|
CREAD
);
currentAttrs_
->
c_cflag
|=
(
CLOCAL
|
CREAD
);
// set communication timeouts
// set communication timeouts
currentAttrs_
->
c_cc
[
VMIN
]
=
kDefaultNumOfBytes
;
currentAttrs_
->
c_cc
[
VMIN
]
=
kDefaultNumOfBytes
;
// converting ms to one-tenths-of-second (sec * 0.1)
currentAttrs_
->
c_cc
[
VTIME
]
=
(
kDefaultReadTimeout
/
100
);
// converting ms to one-tenths-of-second (sec * 0.1)
currentAttrs_
->
c_cc
[
VTIME
]
=
(
kDefaultReadTimeout
/
100
);
// ensure the new attributes take effect immediately
// ensure the new attributes take effect immediately
applyChanges
();
applyChanges
();
}
/*!
...
...
@@ -176,15 +176,15 @@ void TermiosHelper::initTermios()
void
TermiosHelper
::
saveTermios
()
{
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
originalAttrs_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::saveTermios(file: %1) failed when"
\
" getting original port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
originalAttrs_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::saveTermios(file: %1) failed when"
\
" getting original port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
}
/*!
...
...
@@ -193,24 +193,24 @@ void TermiosHelper::saveTermios()
void
TermiosHelper
::
restoreTermios
()
{
if
(
!
originalAttrs_
||
tcsetattr
(
fileDescriptor_
,
TCSANOW
,
originalAttrs_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::restoreTermios(file: %1) failed when resetting "
\
"serial port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
// Block until all written output has been sent from the device.
// Note that this call is simply passed on to the serial device driver.
// See tcsendbreak(3) ("man 3 tcsendbreak") for details.
if
(
tcdrain
(
fileDescriptor_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::restoreTermios(file: %1) failed while waiting for drain: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
if
(
!
originalAttrs_
||
tcsetattr
(
fileDescriptor_
,
TCSANOW
,
originalAttrs_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::restoreTermios(file: %1) failed when resetting "
\
"serial port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
// Block until all written output has been sent from the device.
// Note that this call is simply passed on to the serial device driver.
// See tcsendbreak(3) ("man 3 tcsendbreak") for details.
if
(
tcdrain
(
fileDescriptor_
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::restoreTermios(file: %1) failed while waiting for drain: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
}
/*!
...
...
@@ -219,121 +219,121 @@ void TermiosHelper::restoreTermios()
void
TermiosHelper
::
setBaudRate
(
QPortSettings
::
BaudRate
baudRate
)
{
speed_t
baud
=
B9600
;
speed_t
baud
=
B9600
;
switch
(
baudRate
)
{
switch
(
baudRate
)
{
case
QPortSettings
:
:
BAUDR_50
:
baud
=
B50
;
break
;
baud
=
B50
;
break
;
case
QPortSettings
:
:
BAUDR_75
:
baud
=
B75
;
break
;
baud
=
B75
;
break
;
case
QPortSettings
:
:
BAUDR_110
:
baud
=
B110
;
break
;
baud
=
B110
;
break
;
case
QPortSettings
:
:
BAUDR_134
:
baud
=
B134
;
break
;
baud
=
B134
;
break
;
case
QPortSettings
:
:
BAUDR_150
:
baud
=
B150
;
break
;
baud
=
B150
;
break
;
case
QPortSettings
:
:
BAUDR_200
:
baud
=
B200
;
break
;
baud
=
B200
;
break
;
case
QPortSettings
:
:
BAUDR_300
:
baud
=
B300
;
break
;
baud
=
B300
;
break
;
case
QPortSettings
:
:
BAUDR_600
:
baud
=
B600
;
break
;
baud
=
B600
;
break
;
case
QPortSettings
:
:
BAUDR_1200
:
baud
=
B1200
;
break
;
baud
=
B1200
;
break
;
case
QPortSettings
:
:
BAUDR_1800
:
baud
=
B1800
;
break
;
baud
=
B1800
;
break
;
case
QPortSettings
:
:
BAUDR_2400
:
baud
=
B2400
;
break
;
baud
=
B2400
;
break
;
case
QPortSettings
:
:
BAUDR_4800
:
baud
=
B4800
;
break
;
baud
=
B4800
;
break
;
case
QPortSettings
:
:
BAUDR_9600
:
baud
=
B9600
;
break
;
baud
=
B9600
;
break
;
case
QPortSettings
:
:
BAUDR_19200
:
baud
=
B19200
;
break
;
baud
=
B19200
;
break
;
case
QPortSettings
:
:
BAUDR_38400
:
baud
=
B38400
;
break
;
baud
=
B38400
;
break
;
case
QPortSettings
:
:
BAUDR_57600
:
baud
=
B57600
;
break
;
//case QPortSettings::BAUDR_76800:
// baud = B76800;
// break;
baud
=
B57600
;
break
;
//case QPortSettings::BAUDR_76800:
// baud = B76800;
// break;
case
QPortSettings
:
:
BAUDR_115200
:
baud
=
B115200
;
break
;
case
QPortSettings
:
:
BAUDR_230400
:
baud
=
B115200
;
break
;
case
QPortSettings
:
:
BAUDR_230400
:
#ifdef B230400
baud
=
B230400
;
baud
=
B230400
;
#else
baud
=
(
speed_t
)
230400
;
baud
=
(
speed_t
)
230400
;
#endif
break
;
case
QPortSettings
:
:
BAUDR_460800
:
break
;
case
QPortSettings
:
:
BAUDR_460800
:
#ifdef B460800
baud
=
B460800
;
baud
=
B460800
;
#else
baud
=
(
speed_t
)
460800
;
baud
=
(
speed_t
)
460800
;
#endif
break
;
case
QPortSettings
:
:
BAUDR_500000
:
break
;
case
QPortSettings
:
:
BAUDR_500000
:
#ifdef B500000
baud
=
B500000
;
baud
=
B500000
;
#else
baud
=
(
speed_t
)
500000
;
baud
=
(
speed_t
)
500000
;
#endif
break
;
case
QPortSettings
:
:
BAUDR_576000
:
break
;
case
QPortSettings
:
:
BAUDR_576000
:
#ifdef B576000
baud
=
B576000
;
baud
=
B576000
;
#else
baud
=
(
speed_t
)
576000
;
baud
=
(
speed_t
)
576000
;
#endif
break
;
case
QPortSettings
:
:
BAUDR_921600
:
break
;
case
QPortSettings
:
:
BAUDR_921600
:
#ifdef B921600
baud
=
B921600
;
baud
=
B921600
;
#else
baud
=
(
speed_t
)
921600
;
baud
=
(
speed_t
)
921600
;
#endif
break
;
default:
qWarning
()
<<
"TermiosHelper::setBaudRate("
<<
baudRate
<<
"): "
\
"Unsupported baud rate"
;
}
//#ifdef Q_OS_MAC
// if ( ioctl( fileDescriptor_, IOSSIOSPEED, &baud ) == -1 )
// {
// qCritical() << QString("TermiosHelper::setBaudRate(file: %1) failed: %2(%3)")
// .arg(fileDescriptor_)
// .arg(strerror(errno))
// .arg(errno);
// return false;
// }
//#else
if
(
cfsetspeed
(
currentAttrs_
,
baud
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setBaudRate(file: %1) failed: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
//#endif
break
;
default:
qWarning
()
<<
"TermiosHelper::setBaudRate("
<<
baudRate
<<
"): "
\
"Unsupported baud rate"
;
}
//#ifdef Q_OS_MAC
// if ( ioctl( fileDescriptor_, IOSSIOSPEED, &baud ) == -1 )
// {
// qCritical() << QString("TermiosHelper::setBaudRate(file: %1) failed: %2(%3)")
// .arg(fileDescriptor_)
// .arg(strerror(errno))
// .arg(errno);
// return false;
// }
//#else
if
(
cfsetspeed
(
currentAttrs_
,
baud
)
==
-
1
)
{
qCritical
()
<<
QString
(
"TermiosHelper::setBaudRate(file: %1) failed: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
}
//#endif
}
/*!
...
...
@@ -342,69 +342,73 @@ void TermiosHelper::setBaudRate(QPortSettings::BaudRate baudRate)
QPortSettings
::
BaudRate
TermiosHelper
::
baudRate
()
const
{
speed_t
ibaud
=
cfgetispeed
(
currentAttrs_
);
speed_t
obaud
=
cfgetospeed
(
currentAttrs_
);
speed_t
ibaud
=
cfgetispeed
(
currentAttrs_
);
speed_t
obaud
=
cfgetospeed
(
currentAttrs_
);
(
obaud
==
ibaud
);
(
obaud
==
ibaud
);
Q_ASSERT
(
ibaud
==
obaud
);
Q_ASSERT
(
ibaud
==
obaud
);
switch
(
ibaud
)
{
switch
(
ibaud
)
{
case
B50
:
return
QPortSettings
::
BAUDR_50
;
return
QPortSettings
::
BAUDR_50
;
case
B75
:
return
QPortSettings
::
BAUDR_75
;
return
QPortSettings
::
BAUDR_75
;
case
B110
:
return
QPortSettings
::
BAUDR_110
;
return
QPortSettings
::
BAUDR_110
;
case
B134
:
return
QPortSettings
::
BAUDR_134
;
return
QPortSettings
::
BAUDR_134
;
case
B150
:
return
QPortSettings
::
BAUDR_150
;
return
QPortSettings
::
BAUDR_150
;
case
B200
:
return
QPortSettings
::
BAUDR_200
;
return
QPortSettings
::
BAUDR_200
;
case
B300
:
return
QPortSettings
::
BAUDR_300
;
return
QPortSettings
::
BAUDR_300
;
case
B600
:
return
QPortSettings
::
BAUDR_600
;
return
QPortSettings
::
BAUDR_600
;
case
B1200
:
return
QPortSettings
::
BAUDR_1200
;
return
QPortSettings
::
BAUDR_1200
;
case
B1800
:
return
QPortSettings
::
BAUDR_1800
;
return
QPortSettings
::
BAUDR_1800
;
case
B2400
:
return
QPortSettings
::
BAUDR_2400
;
return
QPortSettings
::
BAUDR_2400
;
case
B4800
:
return
QPortSettings
::
BAUDR_4800
;
return
QPortSettings
::
BAUDR_4800
;
case
B9600
:
return
QPortSettings
::
BAUDR_9600
;
return
QPortSettings
::
BAUDR_9600
;
case
B19200
:
return
QPortSettings
::
BAUDR_19200
;
return
QPortSettings
::
BAUDR_19200
;
case
B38400
:
return
QPortSettings
::
BAUDR_38400
;
return
QPortSettings
::
BAUDR_38400
;
case
B57600
:
return
QPortSettings
::
BAUDR_57600
;
//case B76800:
// return QPortSettings::BAUDR_76800;
return
QPortSettings
::
BAUDR_57600
;
//case B76800:
// return QPortSettings::BAUDR_76800;
case
B115200
:
return
QPortSettings
::
BAUDR_115200
;
#if
defined(Q_OS_LINUX)
return
QPortSettings
::
BAUDR_115200
;
#if
def B230400
case
B230400
:
return
QPortSettings
::
BAUDR_230400
;
// the baud rates listed below are not supported by all UARTs
// and Linux distros
return
QPortSettings
::
BAUDR_230400
;
#endif
#ifdef B460800
case
B460800
:
return
QPortSettings
::
BAUDR_460800
;
return
QPortSettings
::
BAUDR_460800
;
#endif
#ifdef B921600
case
B921600
:
return
QPortSettings
::
BAUDR_921600
;
#endif
#if defined(Q_OS_LINUX)
case
B500000
:
return
QPortSettings
::
BAUDR_500000
;
return
QPortSettings
::
BAUDR_500000
;
case
B576000
:
return
QPortSettings
::
BAUDR_576000
;
case
B921600
:
return
QPortSettings
::
BAUDR_921600
;
return
QPortSettings
::
BAUDR_576000
;
#endif
default:
qWarning
()
<<
"TermiosHelper::baudRate(): Unknown baud rate"
;
}
default:
qWarning
()
<<
"TermiosHelper::baudRate(): Unknown baud rate"
;
}
return
QPortSettings
::
BAUDR_UNKNOWN
;
return
QPortSettings
::
BAUDR_UNKNOWN
;
}
/*!
...
...
@@ -413,29 +417,29 @@ QPortSettings::BaudRate TermiosHelper::baudRate() const
QPortSettings
::
DataBits
TermiosHelper
::
dataBits
()
const
{
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::dataBits(file: %1) failed when"
\
" getting original port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
DB_UNKNOWN
;
}
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS5
)
return
QPortSettings
::
DB_5
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS6
)
return
QPortSettings
::
DB_6
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS7
)
return
QPortSettings
::
DB_7
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS8
)
return
QPortSettings
::
DB_8
;
else
return
QPortSettings
::
DB_UNKNOWN
;
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::dataBits(file: %1) failed when"
\
" getting original port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
DB_UNKNOWN
;
}
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS5
)
return
QPortSettings
::
DB_5
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS6
)
return
QPortSettings
::
DB_6
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS7
)
return
QPortSettings
::
DB_7
;
else
if
(
(
options
.
c_cflag
&
CSIZE
)
==
CS8
)
return
QPortSettings
::
DB_8
;
else
return
QPortSettings
::
DB_UNKNOWN
;
}
...
...
@@ -444,32 +448,32 @@ QPortSettings::DataBits TermiosHelper::dataBits() const
*/
void
TermiosHelper
::
setDataBits
(
QPortSettings
::
DataBits
dataBits
)
{
switch
(
dataBits
)
{
switch
(
dataBits
)
{
/*5 data bits*/
case
QPortSettings
:
:
DB_5
:
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS5
;
break
;
/*6 data bits*/
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS5
;
break
;
/*6 data bits*/
case
QPortSettings
:
:
DB_6
:
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS6
;
break
;
/*7 data bits*/
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS6
;
break
;
/*7 data bits*/
case
QPortSettings
:
:
DB_7
:
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS7
;
break
;
/*8 data bits*/
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS7
;
break
;
/*8 data bits*/
case
QPortSettings
:
:
DB_8
:
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS8
;
break
;
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS8
;
break
;
default:
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS8
;
qWarning
()
<<
"TermiosHelper::setDataBits("
<<
dataBits
<<
"): Unsupported data bits"
;
}
currentAttrs_
->
c_cflag
&=
(
~
CSIZE
);
currentAttrs_
->
c_cflag
|=
CS8
;
qWarning
()
<<
"TermiosHelper::setDataBits("
<<
dataBits
<<
"): Unsupported data bits"
;
}
}
/*!
...
...
@@ -478,27 +482,27 @@ void TermiosHelper::setDataBits(QPortSettings::DataBits dataBits)
QPortSettings
::
Parity
TermiosHelper
::
parity
()
const
{
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::parity(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
PAR_UNKNOWN
;
}
if
(
options
.
c_cflag
&
PARENB
)
{
if
(
options
.
c_cflag
&
PARODD
)
return
QPortSettings
::
PAR_ODD
;
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::parity(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
PAR_UNKNOWN
;
}
if
(
options
.
c_cflag
&
PARENB
)
{
if
(
options
.
c_cflag
&
PARODD
)
return
QPortSettings
::
PAR_ODD
;
else
return
QPortSettings
::
PAR_EVEN
;
}
else
return
QPortSettings
::
PAR_EVEN
;
}
else
return
QPortSettings
::
PAR_NONE
;
return
QPortSettings
::
PAR_NONE
;
}
/*!
...
...
@@ -506,24 +510,24 @@ QPortSettings::Parity TermiosHelper::parity() const
*/
void
TermiosHelper
::
setParity
(
QPortSettings
::
Parity
parity
)
{
switch
(
parity
)
{
switch
(
parity
)
{
/*no parity*/
case
QPortSettings
:
:
PAR_NONE
:
currentAttrs_
->
c_cflag
&=
(
~
PARENB
);
break
;
/*odd parity*/
currentAttrs_
->
c_cflag
&=
(
~
PARENB
);
break
;
/*odd parity*/
case
QPortSettings
:
:
PAR_ODD
:
currentAttrs_
->
c_cflag
|=
(
PARENB
|
PARODD
);
break
;
/*even parity*/
currentAttrs_
->
c_cflag
|=
(
PARENB
|
PARODD
);
break
;
/*even parity*/
case
QPortSettings
:
:
PAR_EVEN
:
currentAttrs_
->
c_cflag
&=
(
~
PARODD
);
currentAttrs_
->
c_cflag
|=
PARENB
;
break
;
currentAttrs_
->
c_cflag
&=
(
~
PARODD
);
currentAttrs_
->
c_cflag
|=
PARENB
;
break
;
default:
currentAttrs_
->
c_cflag
&=
(
~
PARENB
);
qWarning
()
<<
"TermiosHelper::setParity("
<<
parity
<<
"): Unsupported parity"
;
}
currentAttrs_
->
c_cflag
&=
(
~
PARENB
);
qWarning
()
<<
"TermiosHelper::setParity("
<<
parity
<<
"): Unsupported parity"
;
}
}
/*!
...
...
@@ -532,22 +536,22 @@ void TermiosHelper::setParity(QPortSettings::Parity parity)
QPortSettings
::
StopBits
TermiosHelper
::
stopBits
()
const
{
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::stopBits(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
STOP_UNKNOWN
;
}
if
(
options
.
c_cflag
&
CSTOPB
)
return
QPortSettings
::
STOP_2
;
else
return
QPortSettings
::
STOP_1
;
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::stopBits(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
STOP_UNKNOWN
;
}
if
(
options
.
c_cflag
&
CSTOPB
)
return
QPortSettings
::
STOP_2
;
else
return
QPortSettings
::
STOP_1
;
}
/*!
...
...
@@ -555,19 +559,19 @@ QPortSettings::StopBits TermiosHelper::stopBits() const
*/
void
TermiosHelper
::
setStopBits
(
QPortSettings
::
StopBits
stopBits
)
{
switch
(
stopBits
)
{
switch
(
stopBits
)
{
/*one stop bit*/
case
QPortSettings
:
:
STOP_1
:
currentAttrs_
->
c_cflag
&=
(
~
CSTOPB
);
break
;
/*two stop bits*/
currentAttrs_
->
c_cflag
&=
(
~
CSTOPB
);
break
;
/*two stop bits*/
case
QPortSettings
:
:
STOP_2
:
currentAttrs_
->
c_cflag
|=
CSTOPB
;
break
;
currentAttrs_
->
c_cflag
|=
CSTOPB
;
break
;
default:
currentAttrs_
->
c_cflag
&=
(
~
CSTOPB
);
qWarning
()
<<
"TermiosHelper::setStopBits("
<<
stopBits
<<
"): Unsupported stop bits"
;
}
currentAttrs_
->
c_cflag
&=
(
~
CSTOPB
);
qWarning
()
<<
"TermiosHelper::setStopBits("
<<
stopBits
<<
"): Unsupported stop bits"
;
}
}
/*!
...
...
@@ -576,27 +580,27 @@ void TermiosHelper::setStopBits(QPortSettings::StopBits stopBits)
QPortSettings
::
FlowControl
TermiosHelper
::
flowControl
()
const
{
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::flowControl(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
FLOW_UNKNOWN
;
}
if
(
options
.
c_cflag
&
CRTSCTS
)
{
return
QPortSettings
::
FLOW_HARDWARE
;
}
else
{
if
(
options
.
c_iflag
&
IXON
)
return
QPortSettings
::
FLOW_XONXOFF
;
else
return
QPortSettings
::
FLOW_OFF
;
}
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::flowControl(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
QPortSettings
::
FLOW_UNKNOWN
;
}
if
(
options
.
c_cflag
&
CRTSCTS
)
{
return
QPortSettings
::
FLOW_HARDWARE
;
}
else
{
if
(
options
.
c_iflag
&
IXON
)
return
QPortSettings
::
FLOW_XONXOFF
;
else
return
QPortSettings
::
FLOW_OFF
;
}
}
/*!
...
...
@@ -604,26 +608,26 @@ QPortSettings::FlowControl TermiosHelper::flowControl() const
*/
void
TermiosHelper
::
setFlowControl
(
QPortSettings
::
FlowControl
flow
)
{
switch
(
flow
)
{
switch
(
flow
)
{
/*no flow control*/
case
QPortSettings
:
:
FLOW_OFF
:
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
break
;
/*software (XON/XOFF) flow control*/
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
break
;
/*software (XON/XOFF) flow control*/
case
QPortSettings
:
:
FLOW_XONXOFF
:
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
|=
(
IXON
|
IXOFF
|
IXANY
);
break
;
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
|=
(
IXON
|
IXOFF
|
IXANY
);
break
;
case
QPortSettings
:
:
FLOW_HARDWARE
:
currentAttrs_
->
c_cflag
|=
CRTSCTS
;
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
break
;
currentAttrs_
->
c_cflag
|=
CRTSCTS
;
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
break
;
default:
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
qWarning
()
<<
"TermiosHelper::setFlowControl("
<<
flow
<<
"): Unsupported flow control type"
;
}
currentAttrs_
->
c_cflag
&=
(
~
CRTSCTS
);
currentAttrs_
->
c_iflag
&=
(
~
(
IXON
|
IXOFF
|
IXANY
));
qWarning
()
<<
"TermiosHelper::setFlowControl("
<<
flow
<<
"): Unsupported flow control type"
;
}
}
//
...
...
@@ -673,28 +677,28 @@ void TermiosHelper::setFlowControl(QPortSettings::FlowControl flow)
bool
TermiosHelper
::
commTimeouts
(
CommTimeouts
&
commtimeouts
)
const
{
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::commTimeouts(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
commtimeouts
.
PosixVMIN
=
options
.
c_cc
[
VMIN
];
commtimeouts
.
PosixVTIME
=
options
.
c_cc
[
VTIME
];
return
true
;
struct
termios
options
;
// get the current serial port attributes
if
(
tcgetattr
(
fileDescriptor_
,
&
options
)
==
-
1
)
{
qWarning
()
<<
QString
(
"TermiosHelper::commTimeouts(file: %1) failed when getting original "
\
"port attributes: %2(%3)"
)
.
arg
(
fileDescriptor_
)
.
arg
(
strerror
(
errno
))
.
arg
(
errno
);
return
false
;
}
commtimeouts
.
PosixVMIN
=
options
.
c_cc
[
VMIN
];
commtimeouts
.
PosixVTIME
=
options
.
c_cc
[
VTIME
];
return
true
;
}
void
TermiosHelper
::
setCommTimeouts
(
const
CommTimeouts
commtimeouts
)
{
currentAttrs_
->
c_cc
[
VMIN
]
=
(
cc_t
)
commtimeouts
.
PosixVMIN
;
currentAttrs_
->
c_cc
[
VTIME
]
=
(
cc_t
)
commtimeouts
.
PosixVTIME
;
currentAttrs_
->
c_cc
[
VMIN
]
=
(
cc_t
)
commtimeouts
.
PosixVMIN
;
currentAttrs_
->
c_cc
[
VTIME
]
=
(
cc_t
)
commtimeouts
.
PosixVTIME
;
}
}
// namespace
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment