Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

fmu: minor refactoring & fixes

Browse Source 
- initialize rc lost with true
- refactor for simpler downstream code-plugin
- allow for the addition of different binding commands
- fix st24 RC lost logic
This commit is contained in:
Beat Küng
2017-10-11 15:56:20 +02:00
parent bb78931e69
commit 0247d7bdd7
1 changed files with 35 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+15 -5
View File
@@ -412,6 +412,8 @@ PX4FMU::PX4FMU(bool run_as_task) :
// rc input, published to ORB
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
// initialize it as RC lost
_rc_in.rc_lost = true;
// initialize raw_rc values and count
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
@@ -1420,7 +1422,7 @@ PX4FMU::cycle()
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) ||
(_safety_off && _armed.in_esc_calibration_mode);
}
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _armed.armed || _num_disarmed_set > 0 || _armed.in_esc_calibration_mode;
@@ -1430,7 +1432,6 @@ PX4FMU::cycle()
update_pwm_out_state(pwm_on);
}
}
#ifdef RC_SERIAL_PORT
/* vehicle command */
@@ -1440,9 +1441,11 @@ PX4FMU::cycle()
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &cmd);
// Check for a DSM pairing command
if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
// Check for a pairing command
if ((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
if (!_armed.armed) {
if ((int)cmd.param1 == 0) {
// DSM binding command
int dsm_bind_mode = (int)cmd.param2;
int dsm_bind_pulses = 0;
@@ -1458,6 +1461,7 @@ PX4FMU::cycle()
}
ioctl(nullptr, DSM_BIND_START, dsm_bind_pulses);
}
} else {
PX4_WARN("system armed, bind request rejected");
@@ -1611,12 +1615,18 @@ PX4FMU::cycle()
// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
// The only way to detect RC loss is therefore to look at the lost_count.
if (rc_updated && lost_count == 0) {
if (rc_updated) {
if (lost_count == 0) {
// we have a new ST24 frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp,
false, false, frame_drops, st24_rssi);
_rc_scan_locked = true;
} else {
// if the lost count > 0 means that there is an RC loss
_rc_in.rc_lost = true;
}
}
}