From fc099879bea5f099682fa71edfff0b6a59a7dd7d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 2 Mar 2017 09:33:16 -1000 Subject: [PATCH] fmu can be run as a task or as worker thread The new optional option is fmu [task] .... fmu task mode_pwm - start fmu as a task fmu mode_pwm - start fmu as a worker --- src/drivers/px4fmu/fmu.cpp | 1179 ++++++++++++++++++------------------ 1 file changed, 600 insertions(+), 579 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 130236f0f3..3e1fa6a58d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -131,7 +131,7 @@ public: MODE_5CAP, MODE_6CAP, }; - PX4FMU(); + PX4FMU(bool run_as_task); virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -189,6 +189,7 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; + bool _run_as_task; struct work_s _work; int _vehicle_cmd_sub; int _armed_sub; @@ -201,12 +202,11 @@ private: orb_advert_t _outputs_pub; unsigned _num_outputs; int _class_instance; - volatile bool _task_should_exit; + volatile bool _should_exit; int _rcs_fd; uint8_t _rcs_buf[SBUS_BUFFER_SIZE]; static void task_main_trampoline(int argc, char *argv[]); - void task_main(); volatile bool _initialized; bool _throttle_armed; @@ -254,8 +254,8 @@ private: static void cycle_trampoline(void *arg); void cycle(); - void work_start(); - void work_stop(); + int start(); + void stop(); static int control_callback(uintptr_t handle, uint8_t control_group, @@ -321,7 +321,7 @@ PX4FMU *g_fmu; } // namespace -PX4FMU::PX4FMU() : +PX4FMU::PX4FMU(bool run_as_task) : CDev("fmu", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), @@ -329,6 +329,7 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), + _run_as_task(run_as_task), _work{}, _vehicle_cmd_sub(-1), _armed_sub(-1), @@ -341,7 +342,7 @@ PX4FMU::PX4FMU() : _outputs_pub(nullptr), _num_outputs(0), _class_instance(0), - _task_should_exit(false), + _should_exit(false), _rcs_fd(-1), _initialized(false), _throttle_armed(false), @@ -418,7 +419,7 @@ PX4FMU::~PX4FMU() { if (_initialized) { /* tell the task we want it to go away */ - work_stop(); + stop(); int i = 10; @@ -465,35 +466,7 @@ 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, - SCHED_PRIORITY_FAST_DRIVER - 1, - 1280, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); - - /* wait until the task is up and running or has failed */ - while (_task > 0 && _task_should_exit) { - usleep(100); - } - - if (_task < 0) { - return -PX4_ERROR; - } - -#endif - - return OK; -} - -void -PX4FMU::task_main_trampoline(int argc, char *argv[]) -{ - g_fmu->task_main(); + return start(); } void @@ -910,13 +883,38 @@ PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) } -void -PX4FMU::work_start() +int +PX4FMU::start() { -#ifndef PX4FMU_TASK - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, 0); -#endif + + if (!_run_as_task) { + + /* schedule a cycle to start things */ + + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, 0); + + } else { + + /* start the IO interface task */ + + _task = px4_task_spawn_cmd("fmuservo", + SCHED_DEFAULT, + SCHED_PRIORITY_FAST_DRIVER - 1, + 1280, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); + + /* wait until the task is up and running or has failed */ + while (_task > 0 && _should_exit) { + usleep(100); + } + + if (_task < 0) { + return -PX4_ERROR; + } + } + + return PX4_OK; } void @@ -927,6 +925,12 @@ PX4FMU::cycle_trampoline(void *arg) dev->cycle(); } +void +PX4FMU::task_main_trampoline(int argc, char *argv[]) +{ + cycle_trampoline(g_fmu); +} + void PX4FMU::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) @@ -1050,694 +1054,704 @@ 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 + while (!_should_exit) { - 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); -#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); -#ifdef GPIO_PPM_IN - // disable CPPM input by mapping it away from the timer capture input - px4_arch_unconfiggpio(GPIO_PPM_IN); -#endif +# ifdef RF_RADIO_POWER_CONTROL + // 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); +# ifdef 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); - } - - _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); + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + sprintf(pname, "PWM_AUX_TRIM%d", i + 1); + param_find(pname); } + + _initialized = true; } - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; - } + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } - /* 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 + int poll_timeout = 20; - /* this would be bad... */ - if (ret < 0) { - DEVICE_LOG("poll error %d", errno); + if (!_run_as_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; - } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ -// warnx("no PWM: failsafe"); + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); - } else { - perf_begin(_ctl_latency); + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } - /* get controls for required topics */ - unsigned poll_id = 0; - unsigned n_updates = 0; + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } - 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++; + 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); } + } - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + /* check if anything updated */ + poll_timeout = 0; + } + + /* wait for an update */ + int ret = ::poll(_poll_fds, _poll_fds_num, poll_timeout); + + /* 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 */ + // warnx("no PWM: failsafe"); + + } else { + perf_begin(_ctl_latency); + + /* 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++; + } + + 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++; } - poll_id++; - } + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); + /* except thrust to maximum. */ + _controls[i].control[3] = 1.0f; - /* 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; + /* Switch off the PWM limit ramp for the calibration. */ + _pwm_limit.state = PWM_LIMIT_STATE_ON; } } - uint16_t pwm_limited[_max_actuators]; + // only mix if we have a new actuator_controls message + if ((n_updates > 0) && (_mixers != nullptr)) { - /* 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); + 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); - /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ - if (_armed.force_failsafe) { + /* 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 */ for (size_t i = 0; i < num_outputs; i++) { - pwm_limited[i] = _failsafe_pwm[i]; + pwm_output_set(i, pwm_limited[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]; - } - } + /* Trigger all timer's channels in Oneshot mode to fire + * the oneshots with updated values. + */ - /* output to the servos */ - for (size_t i = 0; i < num_outputs; i++) { - pwm_output_set(i, pwm_limited[i]); - } + up_pwm_update(); - // force an update of oneshot channels - up_pwm_force_update(); + publish_pwm_outputs(pwm_limited, num_outputs); + perf_end(_ctl_latency); + } // new actuator_controls message - publish_pwm_outputs(pwm_limited, num_outputs); - perf_end(_ctl_latency); - } // new actuator_controls message + } // poll_fds - } // poll_fds - - _cycle_timestamp = hrt_absolute_time(); + _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(); + } 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); + } } - /* 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); + 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); + } - // 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); + 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); + } } - // 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 */ + /* 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]); - } - - // 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); + if (_report_lock && _rc_scan_locked) { + _report_lock = false; + //warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); } - break; + // read all available data from the serial RC input UART + int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); - 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); + 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) { + } 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) { + 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 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; + 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); } - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_ST24); - } + break; - break; + 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); - 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) { - } 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 - 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)); + 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; + } } - // 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_ST24); } - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_SUMD); - } + break; - 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); - 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) { - } 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; - if (newBytes > 0) { - // parse new data - uint8_t sumd_rssi, rx_count; - bool sumd_failsafe; + rc_updated = false; - 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)); + } - 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)); + // 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) { - // 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_SUMD); } - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_PPM); - } + break; - 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); - case RC_SCAN_PPM: - // skip PPM if it's not supported + } 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); + 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) { + } 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; + // 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 { - // 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 { + 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 if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1000 * 1000)) { - _rc_scan_locked = false; + if (!_run_as_task && !_should_exit) { + + /* + * schedule next cycle + */ + + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); + // USEC2TICK(SCHEDULE_INTERVAL - main_out_latency)); + /* Running a worker. So exit the loop */ + break; + } } - -#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() +void PX4FMU::stop() { - work_cancel(HPWORK, &_work); + /* Signal that we want to stop the task or work. + * + * In the case of work we do not want to reschedule + * to avoid race on cancel + */ + + _should_exit = true; + + if (!_run_as_task) { + work_cancel(HPWORK, &_work); + } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -3072,13 +3086,13 @@ int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz) } int -fmu_start(void) +fmu_start(bool run_as_task) { int ret = OK; if (g_fmu == nullptr) { - g_fmu = new PX4FMU; + g_fmu = new PX4FMU(run_as_task); if (g_fmu == nullptr) { ret = -ENOMEM; @@ -3372,6 +3386,7 @@ extern "C" __EXPORT int fmu_main(int argc, char *argv[]); int fmu_main(int argc, char *argv[]) { + bool run_as_task = false; PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[1]; bool fmu_was_running = false; @@ -3406,7 +3421,13 @@ fmu_main(int argc, char *argv[]) fmu_was_running = (g_fmu != nullptr); - if (fmu_start() != OK) { + run_as_task = !strcmp(verb, "task"); + + if (run_as_task && argc > 2) { + verb = argv[2]; + } + + if (fmu_start(run_as_task) != OK) { errx(1, "failed to start the FMU driver"); }