Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 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
+35 -25
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,16 +1422,15 @@ 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;
/* 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;
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
update_pwm_out_state(pwm_on);
}
update_pwm_out_state(pwm_on);
}
#ifdef RC_SERIAL_PORT
@@ -1440,25 +1441,28 @@ 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) {
int dsm_bind_mode = (int)cmd.param2;
if ((int)cmd.param1 == 0) {
// DSM binding command
int dsm_bind_mode = (int)cmd.param2;
int dsm_bind_pulses = 0;
int dsm_bind_pulses = 0;
if (dsm_bind_mode == 0) {
dsm_bind_pulses = DSM2_BIND_PULSES;
if (dsm_bind_mode == 0) {
dsm_bind_pulses = DSM2_BIND_PULSES;
} else if (dsm_bind_mode == 1) {
dsm_bind_pulses = DSMX_BIND_PULSES;
} else if (dsm_bind_mode == 1) {
dsm_bind_pulses = DSMX_BIND_PULSES;
} else {
dsm_bind_pulses = DSMX8_BIND_PULSES;
} else {
dsm_bind_pulses = DSMX8_BIND_PULSES;
}
ioctl(nullptr, DSM_BIND_START, dsm_bind_pulses);
}
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) {
// 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;
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;
}
}
}