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
1b2ceb4e
Commit
1b2ceb4e
authored
May 21, 2016
by
Lorenz Meier
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
PX4: Do mode switch mapping as done onboard (#3383)
parent
41e5bd1e
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
90 additions
and
14 deletions
+90
-14
PX4SimpleFlightModesController.cc
src/AutoPilotPlugins/PX4/PX4SimpleFlightModesController.cc
+90
-14
No files found.
src/AutoPilotPlugins/PX4/PX4SimpleFlightModesController.cc
View file @
1b2ceb4e
...
...
@@ -75,7 +75,55 @@ void PX4SimpleFlightModesController::_rcChannelsChanged(int channelCount, int pw
return
;
}
int
flightModeReversed
=
pFact
->
rawValue
().
toInt
();
int
pwmRev
=
pFact
->
rawValue
().
toInt
();
pFact
=
getParameterFact
(
-
1
,
QString
(
"RC%1_MIN"
).
arg
(
flightModeChannel
+
1
));
if
(
!
pFact
)
{
#if defined _MSC_VER
qCritical
()
<<
QString
(
"RC%1_MIN"
).
arg
(
flightModeChannel
+
1
)
<<
"Fact is NULL in"
<<
__FILE__
<<
__LINE__
;
#else
qCritical
()
<<
QString
(
"RC%1_MIN"
).
arg
(
flightModeChannel
+
1
)
<<
" Fact is NULL in"
<<
__func__
<<
__FILE__
<<
__LINE__
;
#endif
return
;
}
int
pwmMin
=
pFact
->
rawValue
().
toInt
();
pFact
=
getParameterFact
(
-
1
,
QString
(
"RC%1_MAX"
).
arg
(
flightModeChannel
+
1
));
if
(
!
pFact
)
{
#if defined _MSC_VER
qCritical
()
<<
QString
(
"RC%1_MAX"
).
arg
(
flightModeChannel
+
1
)
<<
"Fact is NULL in"
<<
__FILE__
<<
__LINE__
;
#else
qCritical
()
<<
QString
(
"RC%1_MAX"
).
arg
(
flightModeChannel
+
1
)
<<
" Fact is NULL in"
<<
__func__
<<
__FILE__
<<
__LINE__
;
#endif
return
;
}
int
pwmMax
=
pFact
->
rawValue
().
toInt
();
pFact
=
getParameterFact
(
-
1
,
QString
(
"RC%1_TRIM"
).
arg
(
flightModeChannel
+
1
));
if
(
!
pFact
)
{
#if defined _MSC_VER
qCritical
()
<<
QString
(
"RC%1_TRIM"
).
arg
(
flightModeChannel
+
1
)
<<
"Fact is NULL in"
<<
__FILE__
<<
__LINE__
;
#else
qCritical
()
<<
QString
(
"RC%1_TRIM"
).
arg
(
flightModeChannel
+
1
)
<<
" Fact is NULL in"
<<
__func__
<<
__FILE__
<<
__LINE__
;
#endif
return
;
}
int
pwmTrim
=
pFact
->
rawValue
().
toInt
();
pFact
=
getParameterFact
(
-
1
,
QString
(
"RC%1_DZ"
).
arg
(
flightModeChannel
+
1
));
if
(
!
pFact
)
{
#if defined _MSC_VER
qCritical
()
<<
QString
(
"RC%1_DZ"
).
arg
(
flightModeChannel
+
1
)
<<
"Fact is NULL in"
<<
__FILE__
<<
__LINE__
;
#else
qCritical
()
<<
QString
(
"RC%1_DZ"
).
arg
(
flightModeChannel
+
1
)
<<
" Fact is NULL in"
<<
__func__
<<
__FILE__
<<
__LINE__
;
#endif
return
;
}
int
pwmDz
=
pFact
->
rawValue
().
toInt
();
if
(
flightModeChannel
<
0
||
flightModeChannel
>
channelCount
)
{
return
;
...
...
@@ -83,24 +131,52 @@ void PX4SimpleFlightModesController::_rcChannelsChanged(int channelCount, int pw
_activeFlightMode
=
0
;
int
channelValue
=
pwmValues
[
flightModeChannel
];
if
(
channelValue
!=
-
1
)
{
bool
found
=
false
;
int
rgThreshold
[]
=
{
1200
,
1360
,
1490
,
1620
,
1900
};
for
(
int
i
=
0
;
i
<
5
;
i
++
)
{
if
(
channelValue
<=
rgThreshold
[
i
])
{
_activeFlightMode
=
i
+
1
;
found
=
true
;
break
;
}
}
/* the half width of the range of a slot is the total range
* divided by the number of slots, again divided by two
*/
const
unsigned
num_slots
=
6
;
const
float
slot_width_half
=
2.0
f
/
num_slots
/
2.0
f
;
/* min is -1, max is +1, range is 2. We offset below min and max */
const
float
slot_min
=
-
1.0
f
-
0.05
f
;
const
float
slot_max
=
1.0
f
+
0.05
f
;
/* the slot gets mapped by first normalizing into a 0..1 interval using min
* and max. Then the right slot is obtained by multiplying with the number of
* slots. And finally we add half a slot width to ensure that integer rounding
* will take us to the correct final index.
*/
float
calibrated_value
;
if
(
!
found
)
{
_activeFlightMode
=
6
;
if
(
channelValue
>
(
pwmTrim
+
pwmDz
))
{
calibrated_value
=
(
channelValue
-
pwmTrim
-
pwmDz
)
/
(
float
)(
pwmMax
-
pwmTrim
-
pwmDz
);
}
else
if
(
channelValue
<
(
pwmTrim
-
pwmDz
))
{
calibrated_value
=
(
channelValue
-
pwmTrim
+
pwmDz
)
/
(
float
)(
pwmTrim
-
pwmMin
-
pwmDz
);
}
else
{
/* in the configured dead zone, output zero */
calibrated_value
=
0.0
f
;
}
if
(
flightModeReversed
==
-
1
)
{
_activeFlightMode
=
6
-
(
_activeFlightMode
-
1
);
calibrated_value
*=
pwmRev
;
_activeFlightMode
=
(((((
calibrated_value
-
slot_min
)
*
num_slots
)
+
slot_width_half
)
/
(
slot_max
-
slot_min
))
+
(
1.0
f
/
num_slots
));
if
(
_activeFlightMode
>=
static_cast
<
int
>
(
num_slots
))
{
_activeFlightMode
=
num_slots
-
1
;
}
// move to 1-based index
_activeFlightMode
++
;
}
emit
activeFlightModeChanged
(
_activeFlightMode
);
...
...
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