Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
CPS-VO
PX4-Firmware
Commits
10dfd2ba
Commit
10dfd2ba
authored
Apr 20, 2013
by
Anton Babushkin
Browse files
dt calculation bug fixed
parent
99ed9be4
Changes
1
Hide whitespace changes
Inline
Side-by-side
apps/multirotor_pos_control/multirotor_pos_control.c
View file @
10dfd2ba
...
...
@@ -214,6 +214,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
status
.
state_machine
==
SYSTEM_STATE_AUTO
);
hrt_abstime
t
=
hrt_absolute_time
();
float
dt
;
if
(
t_prev
!=
0
)
{
dt
=
(
t
-
t_prev
)
*
0
.
000001
f
;
}
else
{
dt
=
0
.
0
f
;
}
t_prev
=
t
;
if
(
act
)
{
orb_copy
(
ORB_ID
(
manual_control_setpoint
),
manual_sub
,
&
manual
);
orb_copy
(
ORB_ID
(
vehicle_attitude
),
att_sub
,
&
att
);
...
...
@@ -223,7 +231,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
orb_copy
(
ORB_ID
(
vehicle_local_position_setpoint
),
local_pos_sp_sub
,
&
local_pos_sp
);
}
hrt_abstime
t
=
hrt_absolute_time
();
if
(
reset_sp_alt
)
{
reset_sp_alt
=
false
;
local_pos_sp
.
z
=
local_pos
.
z
;
...
...
@@ -242,12 +249,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
mavlink_log_info
(
mavlink_fd
,
str
);
}
float
dt
;
if
(
t_prev
!=
0
)
{
dt
=
(
t
-
t_prev
)
*
0
.
000001
f
;
}
else
{
dt
=
0
.
0
f
;
}
float
err_linear_limit
=
params
.
alt_d
/
params
.
alt_p
*
params
.
alt_rate_max
;
if
(
status
.
flag_control_manual_enabled
)
{
...
...
@@ -303,7 +304,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
}
/* publish new attitude setpoint */
orb_publish
(
ORB_ID
(
vehicle_attitude_setpoint
),
att_sp_pub
,
&
att_sp
);
t_prev
=
t
;
}
else
{
reset_sp_alt
=
true
;
reset_sp_pos
=
true
;
...
...
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