fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 12:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Change arming transfer to only set the register if the local configuration changed. Move its write operation to the fast rate so that arming / disarming is instantaneous

Browse Source
This commit is contained in:
Lorenz Meier
2016-01-17 13:45:44 +01:00
parent f485b60f57
commit ab65a55fbf
1 changed files with 25 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+25 -12
View File
@@ -273,6 +273,7 @@ private:
/* cached IO state */
uint16_t _status; ///< Various IO status flags
uint16_t _alarms; ///< Various IO alarms
uint16_t _last_written_arming; ///< the last written arming state reg
/* subscribed topics */
int _t_actuator_controls_0; ///< actuator controls group 0 topic
@@ -509,6 +510,7 @@ PX4IO::PX4IO(device::Device *interface) :
_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
_status(0),
_alarms(0),
_last_written_arming(0),
_t_actuator_controls_0(-1),
_t_actuator_controls_1(-1),
_t_actuator_controls_2(-1),
@@ -977,7 +979,7 @@ PX4IO::task_main()
}
if (now >= poll_last + IO_POLL_INTERVAL) {
/* run at 50Hz */
/* run at 50-250Hz */
poll_last = now;
/* pull status and alarms from IO */
@@ -988,16 +990,6 @@ PX4IO::task_main()
/* fetch PWM outputs from IO */
io_publish_pwm_outputs();
}
if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
/* run at 5Hz */
orb_check_last = now;
/* try to claim the MAVLink log FD */
if (_mavlink_fd < 0) {
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
}
/* check updates on uORB topics and handle it */
bool updated = false;
@@ -1012,6 +1004,19 @@ PX4IO::task_main()
if (updated) {
io_set_arming_state();
}
}
if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
/* run at 5Hz */
orb_check_last = now;
/* try to claim the MAVLink log FD */
if (_mavlink_fd < 0) {
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
}
/* check updates on uORB topics and handle it */
bool updated = false;
/* vehicle command */
orb_check(_t_vehicle_command, &updated);
@@ -1350,7 +1355,15 @@ PX4IO::io_set_arming_state()
}
}
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
uint16_t new_arming_state = set;
new_arming_state &= ~(clear);
if (_last_written_arming != new_arming_state) {
_last_written_arming = new_arming_state;
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}
return 0;
}
int