diff --git a/src/drivers/boards/auav-x21/board_config.h b/src/drivers/boards/auav-x21/board_config.h index 6bc392043f..91f2d7f575 100644 --- a/src/drivers/boards/auav-x21/board_config.h +++ b/src/drivers/boards/auav-x21/board_config.h @@ -211,6 +211,9 @@ #define BOARD_NAME "AUAV_X21" +// Run the FMU task on its own thread to reduce jitter +#define PX4FMU_TASK + /* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC * system_power interface, and therefore provides the true logic * GPIO BOARD_ADC_xxxx macros. diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index f86d904aea..02b8df95ba 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -275,6 +275,9 @@ #define BOARD_NAME "PX4FMU_V4" +// Run the FMU task on its own thread to reduce jitter +#define PX4FMU_TASK + /* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) * this board support the ADC system_power interface, and therefore * provides the true logic GPIO BOARD_ADC_xxxx macros. diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3e3a89662d..1bd1b05ab5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -184,12 +184,12 @@ private: static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; - bool _oneshot_mode; unsigned _pwm_default_rate; unsigned _pwm_alt_rate; uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; + struct work_s _work; int _vehicle_cmd_sub; int _armed_sub; int _param_sub; @@ -254,6 +254,7 @@ private: static void cycle_trampoline(void *arg); void cycle(); + void work_start(); void work_stop(); static int control_callback(uintptr_t handle, @@ -323,12 +324,12 @@ PX4FMU *g_fmu; PX4FMU::PX4FMU() : CDev("fmu", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), - _oneshot_mode(false), _pwm_default_rate(50), _pwm_alt_rate(50), _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), + _work{}, _vehicle_cmd_sub(-1), _armed_sub(-1), _param_sub(-1), @@ -464,6 +465,9 @@ PX4FMU::init() _safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); +#ifndef PX4FMU_TASK + work_start(); +#else /* start the IO interface task */ _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, @@ -481,6 +485,8 @@ PX4FMU::init() return -PX4_ERROR; } +#endif + return OK; } @@ -868,6 +874,15 @@ PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) } +void +PX4FMU::work_start() +{ +#ifndef PX4FMU_TASK + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, 0); +#endif +} + void PX4FMU::cycle_trampoline(void *arg) { @@ -999,633 +1014,689 @@ PX4FMU::update_pwm_out_state(bool on) up_pwm_servo_arm(on); } +#ifndef PX4FMU_TASK +void +PX4FMU::cycle() +{ +#else void PX4FMU::task_main() { while (!_task_should_exit) { +#endif - if (!_initialized) { - /* force a reset of the update rate */ - _current_update_rate = 0; + if (!_initialized) { + /* force a reset of the update rate */ + _current_update_rate = 0; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - _adc_sub = orb_subscribe(ORB_ID(adc_report)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + _adc_sub = orb_subscribe(ORB_ID(adc_report)); - /* initialize PWM limit lib */ - pwm_limit_init(&_pwm_limit); + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); - update_pwm_rev_mask(); + update_pwm_rev_mask(); #ifdef RC_SERIAL_PORT #ifdef RF_RADIO_POWER_CONTROL - // power radio on - RF_RADIO_POWER_CONTROL(true); + // power radio on + RF_RADIO_POWER_CONTROL(true); #endif - _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - // dsm_init sets some file static variables and returns a file descriptor - _rcs_fd = dsm_init(RC_SERIAL_PORT); - // assume SBUS input - sbus_config(_rcs_fd, false); + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + // dsm_init sets some file static variables and returns a file descriptor + _rcs_fd = dsm_init(RC_SERIAL_PORT); + // assume SBUS input + sbus_config(_rcs_fd, false); #ifdef GPIO_PPM_IN - // disable CPPM input by mapping it away from the timer capture input - px4_arch_unconfiggpio(GPIO_PPM_IN); + // disable CPPM input by mapping it away from the timer capture input + px4_arch_unconfiggpio(GPIO_PPM_IN); #endif #endif - param_find("MOT_SLEW_MAX"); - param_find("THR_MDL_FAC"); + param_find("MOT_SLEW_MAX"); + param_find("THR_MDL_FAC"); - for (unsigned i = 0; i < _max_actuators; i++) { - char pname[16]; - sprintf(pname, "PWM_AUX_TRIM%d", i + 1); - param_find(pname); + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + sprintf(pname, "PWM_AUX_TRIM%d", i + 1); + param_find(pname); + } + + _initialized = true; + } + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } + +#ifndef PX4FMU_TASK + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } + + PX4_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); } - - _initialized = true; } - if (_groups_subscribed != _groups_required) { - subscribe(); - _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; - } + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } - /* wait for an update */ - int ret = ::poll(_poll_fds, _poll_fds_num, 20); + /* check if anything updated */ + int ret = ::poll(_poll_fds, _poll_fds_num, 0); +#else + /* wait for an update */ + int ret = ::poll(_poll_fds, _poll_fds_num, 20); +#endif - /* this would be bad... */ - if (ret < 0) { - DEVICE_LOG("poll error %d", errno); + /* this would be bad... */ + if (ret < 0) { + DEVICE_LOG("poll error %d", errno); - } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); - } else { - perf_begin(_ctl_latency); + } else { + perf_begin(_ctl_latency); - /* get controls for required topics */ - unsigned poll_id = 0; - unsigned n_updates = 0; + /* get controls for required topics */ + unsigned poll_id = 0; + unsigned n_updates = 0; - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { - if (i == 0) { - n_updates++; - } + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + if (i == 0) { + n_updates++; + } - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); #if defined(DEBUG_BUILD) - static int main_out_latency = 0; - static int sum_latency = 0; - static uint64_t last_cycle_time = 0; + static int main_out_latency = 0; + static int sum_latency = 0; + static uint64_t last_cycle_time = 0; - if (i == 0) { - uint64_t now = hrt_absolute_time(); - uint64_t latency = now - _controls[i].timestamp; + if (i == 0) { + uint64_t now = hrt_absolute_time(); + uint64_t latency = now - _controls[i].timestamp; - if (latency > main_out_latency) { main_out_latency = latency; } + if (latency > main_out_latency) { main_out_latency = latency; } - sum_latency += latency; + sum_latency += latency; - if ((now - last_cycle_time) >= 1000000) { - last_cycle_time = now; - PX4_DEBUG("pwm max latency: %d, avg: %5.3f", main_out_latency, (double)(sum_latency / 100.0)); - main_out_latency = latency; - sum_latency = 0; - } + if ((now - last_cycle_time) >= 1000000) { + last_cycle_time = now; + PX4_DEBUG("pwm max latency: %d, avg: %5.3f", main_out_latency, (double)(sum_latency / 100.0)); + main_out_latency = latency; + sum_latency = 0; } - -#endif } - poll_id++; +#endif } - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { + poll_id++; + } - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { - /* except thrust to maximum. */ - _controls[i].control[3] = 1.0f; + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); - /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; + /* except thrust to maximum. */ + _controls[i].control[3] = 1.0f; + + /* Switch off the PWM limit ramp for the calibration. */ + _pwm_limit.state = PWM_LIMIT_STATE_ON; + } + } + + // only mix if we have a new actuator_controls message + if ((n_updates > 0) && (_mixers != nullptr)) { + + size_t num_outputs; + + switch (_mode) { + case MODE_1PWM: + num_outputs = 1; + break; + + case MODE_2PWM: + case MODE_2PWM2CAP: + num_outputs = 2; + break; + + case MODE_3PWM: + case MODE_3PWM1CAP: + num_outputs = 3; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + case MODE_8PWM: + num_outputs = 8; + break; + + default: + num_outputs = 0; + break; + } + + hrt_abstime now = hrt_absolute_time(); + float dt = (now - _time_last_mix) / 1e6f; + _time_last_mix = now; + + if (dt < 0.0001f) { + dt = 0.0001f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + if (_mot_t_max > FLT_EPSILON) { + // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle + // factor 2 is needed because actuator ouputs are in the range [-1,1] + float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; + _mixers->set_max_delta_out_once(delta_out_max); + } + + if (_thr_mdl_fac > FLT_EPSILON) { + _mixers->set_thrust_factor(_thr_mdl_fac); + } + + /* do mixing */ + float outputs[_max_actuators]; + num_outputs = _mixers->mix(outputs, num_outputs, NULL); + + /* publish mixer status */ + multirotor_motor_limits_s multirotor_motor_limits = {}; + multirotor_motor_limits.saturation_status = _mixers->get_saturation_status(); + + if (_to_mixer_status == nullptr) { + /* publish limits */ + int instance = _class_instance; + _to_mixer_status = orb_advertise_multi(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits, &instance, + ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits); + + } + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { + if (i >= num_outputs) { + outputs[i] = NAN_VALUE; } } - // only mix if we have a new actuator_controls message - if ((n_updates > 0) && (_mixers != nullptr)) { + uint16_t pwm_limited[_max_actuators]; - size_t num_outputs; - - switch (_mode) { - case MODE_1PWM: - num_outputs = 1; - break; - - case MODE_2PWM: - case MODE_2PWM2CAP: - num_outputs = 2; - break; - - case MODE_3PWM: - case MODE_3PWM1CAP: - num_outputs = 3; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_6PWM: - num_outputs = 6; - break; - - case MODE_8PWM: - num_outputs = 8; - break; - - default: - num_outputs = 0; - break; - } - - hrt_abstime now = hrt_absolute_time(); - float dt = (now - _time_last_mix) / 1e6f; - _time_last_mix = now; - - if (dt < 0.0001f) { - dt = 0.0001f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - if (_mot_t_max > FLT_EPSILON) { - // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator ouputs are in the range [-1,1] - float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; - _mixers->set_max_delta_out_once(delta_out_max); - } - - if (_thr_mdl_fac > FLT_EPSILON) { - _mixers->set_thrust_factor(_thr_mdl_fac); - } - - /* do mixing */ - float outputs[_max_actuators]; - num_outputs = _mixers->mix(outputs, num_outputs, NULL); - - /* publish mixer status */ - multirotor_motor_limits_s multirotor_motor_limits = {}; - multirotor_motor_limits.saturation_status = _mixers->get_saturation_status(); - - if (_to_mixer_status == nullptr) { - /* publish limits */ - int instance = _class_instance; - _to_mixer_status = orb_advertise_multi(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits, &instance, - ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits); - - } - - /* disable unused ports by setting their output to NaN */ - for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { - if (i >= num_outputs) { - outputs[i] = NAN_VALUE; - } - } - - uint16_t pwm_limited[_max_actuators]; - - /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, + _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); - /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ - if (_armed.force_failsafe) { - for (size_t i = 0; i < num_outputs; i++) { - pwm_limited[i] = _failsafe_pwm[i]; - } - } - - /* overwrite outputs in case of lockdown with disarmed PWM values */ - if (_armed.lockdown || _armed.manual_lockdown) { - for (size_t i = 0; i < num_outputs; i++) { - pwm_limited[i] = _disarmed_pwm[i]; - } - } - - /* output to the servos */ + /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ + if (_armed.force_failsafe) { for (size_t i = 0; i < num_outputs; i++) { - pwm_output_set(i, pwm_limited[i]); + pwm_limited[i] = _failsafe_pwm[i]; } + } - // force an update of oneshot channels - up_pwm_force_update(); + /* overwrite outputs in case of lockdown with disarmed PWM values */ + if (_armed.lockdown || _armed.manual_lockdown) { + for (size_t i = 0; i < num_outputs; i++) { + pwm_limited[i] = _disarmed_pwm[i]; + } + } - publish_pwm_outputs(pwm_limited, num_outputs); - perf_end(_ctl_latency); - } // new actuator_controls message + /* output to the servos */ + for (size_t i = 0; i < num_outputs; i++) { + pwm_output_set(i, pwm_limited[i]); + } - } // poll_fds + // force an update of oneshot channels + up_pwm_force_update(); - _cycle_timestamp = hrt_absolute_time(); + publish_pwm_outputs(pwm_limited, num_outputs); + perf_end(_ctl_latency); + } // new actuator_controls message + + } // poll_fds + + _cycle_timestamp = hrt_absolute_time(); #ifdef GPIO_BTN_SAFETY - if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { - _last_safety_check = _cycle_timestamp; + if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { + _last_safety_check = _cycle_timestamp; - /** - * Get and handle the safety status at 10Hz - */ - struct safety_s safety = {}; + /** + * Get and handle the safety status at 10Hz + */ + struct safety_s safety = {}; - if (_safety_disabled) { - _safety_off = true; + if (_safety_disabled) { + _safety_off = true; - } else { - /* read safety switch input and control safety switch LED at 10Hz */ - safety_check_button(); - } - - /* Make the safety button flash anyway, no matter if it's used or not. */ - flash_safety_button(); - - safety.timestamp = hrt_absolute_time(); - - if (_safety_off) { - safety.safety_off = true; - safety.safety_switch_available = true; - - } else { - safety.safety_off = false; - safety.safety_switch_available = true; - } - - /* lazily publish the safety status */ - if (_to_safety != nullptr) { - orb_publish(ORB_ID(safety), _to_safety, &safety); - - } else { - int instance = _class_instance; - _to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT); - } + } else { + /* read safety switch input and control safety switch LED at 10Hz */ + safety_check_button(); } + /* Make the safety button flash anyway, no matter if it's used or not. */ + flash_safety_button(); + + safety.timestamp = hrt_absolute_time(); + + if (_safety_off) { + safety.safety_off = true; + safety.safety_switch_available = true; + + } else { + safety.safety_off = false; + safety.safety_switch_available = true; + } + + /* lazily publish the safety status */ + if (_to_safety != nullptr) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + + } else { + int instance = _class_instance; + _to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT); + } + } + #endif - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* Update the armed status and check that we're not locked down. - * 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 the armed status and check that we're not locked down. + * 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 - /* vehicle command */ - orb_check(_vehicle_cmd_sub, &updated); + /* vehicle command */ + orb_check(_vehicle_cmd_sub, &updated); - if (updated) { - 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)) { - dsm_bind_ioctl((int)cmd.param2); - } + if (updated) { + 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)) { + dsm_bind_ioctl((int)cmd.param2); } + } + #endif - orb_check(_param_sub, &updated); + orb_check(_param_sub, &updated); - if (updated) { - parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - update_pwm_rev_mask(); - update_pwm_trims(); + update_pwm_rev_mask(); + update_pwm_trims(); - int32_t dsm_bind_val; - param_t param_handle; + int32_t dsm_bind_val; + param_t param_handle; - /* see if bind parameter has been set, and reset it to -1 */ - param_get(param_handle = param_find("RC_DSM_BIND"), &dsm_bind_val); + /* see if bind parameter has been set, and reset it to -1 */ + param_get(param_handle = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val > -1) { - dsm_bind_ioctl(dsm_bind_val); - dsm_bind_val = -1; - param_set(param_handle, &dsm_bind_val); - } - - // maximum motor slew rate parameter - param_handle = param_find("MOT_SLEW_MAX"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_mot_t_max); - } - - // thrust to pwm modelling factor - param_handle = param_find("THR_MDL_FAC"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_thr_mdl_fac); - } + if (dsm_bind_val > -1) { + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; + param_set(param_handle, &dsm_bind_val); } - /* update ADC sampling */ + // maximum motor slew rate parameter + param_handle = param_find("MOT_SLEW_MAX"); + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &_mot_t_max); + } + + // thrust to pwm modelling factor + param_handle = param_find("THR_MDL_FAC"); + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &_thr_mdl_fac); + } + } + + /* update ADC sampling */ #ifdef ADC_RC_RSSI_CHANNEL - orb_check(_adc_sub, &updated); + orb_check(_adc_sub, &updated); - if (updated) { + if (updated) { - struct adc_report_s adc; - orb_copy(ORB_ID(adc_report), _adc_sub, &adc); - const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]); + struct adc_report_s adc; + orb_copy(ORB_ID(adc_report), _adc_sub, &adc); + const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]); - for (unsigned i = 0; i < adc_chans; i++) { - if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) { + for (unsigned i = 0; i < adc_chans; i++) { + if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) { - if (_analog_rc_rssi_volt < 0.0f) { - _analog_rc_rssi_volt = adc.channel_value[i]; - } + if (_analog_rc_rssi_volt < 0.0f) { + _analog_rc_rssi_volt = adc.channel_value[i]; + } - _analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f; + _analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f; - /* only allow this to be used if we see a high RSSI once */ - if (_analog_rc_rssi_volt > 2.5f) { - _analog_rc_rssi_stable = true; - } + /* only allow this to be used if we see a high RSSI once */ + if (_analog_rc_rssi_volt > 2.5f) { + _analog_rc_rssi_stable = true; } } } + } #endif - bool rc_updated = false; + bool rc_updated = false; #ifdef RC_SERIAL_PORT - // This block scans for a supported serial RC input and locks onto the first one found - // Scan for 300 msec, then switch protocol - constexpr hrt_abstime rc_scan_max = 300 * 1000; + // This block scans for a supported serial RC input and locks onto the first one found + // Scan for 300 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 300 * 1000; - bool sbus_failsafe, sbus_frame_drop; - unsigned frame_drops; - bool dsm_11_bit; + bool sbus_failsafe, sbus_frame_drop; + unsigned frame_drops; + bool dsm_11_bit; - if (_report_lock && _rc_scan_locked) { - _report_lock = false; - //warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + if (_report_lock && _rc_scan_locked) { + _report_lock = false; + //warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + } + + // read all available data from the serial RC input UART + int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + + switch (_rc_scan_state) { + case RC_SCAN_SBUS: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for SBUS + sbus_config(_rcs_fd, false); + rc_io_invert(true); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // parse new data + if (newBytes > 0) { + rc_updated = sbus_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, + &sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + // we have a new SBUS frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; + fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, + sbus_frame_drop, sbus_failsafe, frame_drops); + _rc_scan_locked = true; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_DSM); } - // read all available data from the serial RC input UART - int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + break; - switch (_rc_scan_state) { - case RC_SCAN_SBUS: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for SBUS - sbus_config(_rcs_fd, false); - rc_io_invert(true); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // parse new data - if (newBytes > 0) { - rc_updated = sbus_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, - &sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (rc_updated) { - // we have a new SBUS frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; - fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, - sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_DSM); - } - - break; - - case RC_SCAN_DSM: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; + case RC_SCAN_DSM: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; // // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false); + dsm_config(_rcs_fd); + rc_io_invert(false); - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - if (newBytes > 0) { - // parse new data - rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + if (newBytes > 0) { + // parse new data + rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, + &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); - if (rc_updated) { - // we have a new DSM frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; - fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, - false, false, frame_drops); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_ST24); - } - - break; - - case RC_SCAN_ST24: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - if (newBytes > 0) { - // parse new data - uint8_t st24_rssi, lost_count; - - rc_updated = false; - - for (unsigned i = 0; i < (unsigned)newBytes; i++) { - /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; - rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, - &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); - } - - // 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; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_SUMD); - } - - break; - - case RC_SCAN_SUMD: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - if (newBytes > 0) { - // parse new data - uint8_t sumd_rssi, rx_count; - bool sumd_failsafe; - - rc_updated = false; - - for (unsigned i = 0; i < (unsigned)newBytes; i++) { - /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; - rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, - &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); - } - - if (rc_updated) { - // we have a new SUMD frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; - fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, - false, sumd_failsafe, frame_drops, sumd_rssi); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_PPM); - } - - break; - - case RC_SCAN_PPM: - // skip PPM if it's not supported -#ifdef HRT_PPM_CHANNEL - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure timer input pin for CPPM - px4_arch_configgpio(GPIO_PPM_IN); - rc_io_invert(false); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) - && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - rc_updated = true; - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, - false, false, 0); + if (rc_updated) { + // we have a new DSM frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; + fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, + false, false, frame_drops); _rc_scan_locked = true; - _rc_in.rc_ppm_frame_length = ppm_frame_length; - _rc_in.timestamp_last_signal = ppm_last_valid_decode; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_ST24); + } + + break; + + case RC_SCAN_ST24: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for DSM + dsm_config(_rcs_fd); + rc_io_invert(false); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t st24_rssi, lost_count; + + rc_updated = false; + + for (unsigned i = 0; i < (unsigned)newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, + &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); } - } else { - // disable CPPM input by mapping it away from the timer capture input - px4_arch_unconfiggpio(GPIO_PPM_IN); - // Scan the next protocol - set_rc_scan_state(RC_SCAN_SBUS); + // 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; + } } + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SUMD); + } + + break; + + case RC_SCAN_SUMD: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for DSM + dsm_config(_rcs_fd); + rc_io_invert(false); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t sumd_rssi, rx_count; + bool sumd_failsafe; + + rc_updated = false; + + for (unsigned i = 0; i < (unsigned)newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + sumd_rssi = RC_INPUT_RSSI_MAX; + rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, + &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); + } + + if (rc_updated) { + // we have a new SUMD frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; + fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, + false, sumd_failsafe, frame_drops, sumd_rssi); + _rc_scan_locked = true; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_PPM); + } + + break; + + case RC_SCAN_PPM: + // skip PPM if it's not supported +#ifdef HRT_PPM_CHANNEL + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure timer input pin for CPPM + px4_arch_configgpio(GPIO_PPM_IN); + rc_io_invert(false); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) + && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, + false, false, 0); + _rc_scan_locked = true; + _rc_in.rc_ppm_frame_length = ppm_frame_length; + _rc_in.timestamp_last_signal = ppm_last_valid_decode; + } + + } else { + // disable CPPM input by mapping it away from the timer capture input + px4_arch_unconfiggpio(GPIO_PPM_IN); + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SBUS); + } + #else // skip PPM if it's not supported - set_rc_scan_state(RC_SCAN_SBUS); + set_rc_scan_state(RC_SCAN_SBUS); #endif // HRT_PPM_CHANNEL - break; - } + break; + } #else // RC_SERIAL_PORT not defined #ifdef HRT_PPM_CHANNEL - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) - && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - rc_updated = true; - fill_rc_in(ppm_decoded_channels, ppm_buffer, hrt_absolute_time(), - false, false, 0); - } + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) + && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + fill_rc_in(ppm_decoded_channels, ppm_buffer, hrt_absolute_time(), + false, false, 0); + } #endif // HRT_PPM_CHANNEL #endif // RC_SERIAL_PORT - if (rc_updated) { - /* lazily advertise on first publication */ - if (_to_input_rc == nullptr) { - int instance = _class_instance; - _to_input_rc = orb_advertise_multi(ORB_ID(input_rc), &_rc_in, &instance, ORB_PRIO_DEFAULT); + if (rc_updated) { + /* lazily advertise on first publication */ + if (_to_input_rc == nullptr) { + int instance = _class_instance; + _to_input_rc = orb_advertise_multi(ORB_ID(input_rc), &_rc_in, &instance, ORB_PRIO_DEFAULT); - } else { - orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in); - } - - } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1000 * 1000)) { - _rc_scan_locked = false; + } else { + orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in); } + + } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1000 * 1000)) { + _rc_scan_locked = false; } + +#ifdef PX4FMU_TASK +} + +#else + /* + * schedule next cycle + */ + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); +// USEC2TICK(SCHEDULE_INTERVAL - main_out_latency)); + +#endif } void PX4FMU::work_stop()