diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a0d43316b731341bfedf74b1ad473390866126ec..43f80912d89b40b61c761507f59a92a9891b6c1e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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,24 +1441,27 @@ 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; + } } }