From 74607242422269e4d492437dafeaddf943dd5f01 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 4 Aug 2015 11:37:08 +0200 Subject: [PATCH 01/12] fix current scaling for mavlink message --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cb56331e95..0b6dfcab5f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -537,7 +537,7 @@ protected: msg.onboard_control_sensors_health = status.onboard_control_sensors_health; msg.load = status.load * 1000.0f; msg.voltage_battery = status.battery_voltage * 1000.0f; - msg.current_battery = status.battery_current / 10.0f; + msg.current_battery = status.battery_current * 100.0f; msg.drop_rate_comm = status.drop_rate_comm; msg.errors_comm = status.errors_comm; msg.errors_count1 = status.errors_count1; @@ -562,7 +562,7 @@ protected: bat_msg.voltages[i] = 0; } } - bat_msg.current_battery = status.battery_current / 10.0f; + bat_msg.current_battery = status.battery_current * 100.0f; bat_msg.current_consumed = status.battery_discharged_mah; bat_msg.energy_consumed = -1.0f; bat_msg.battery_remaining = (status.battery_voltage > 0) ? From fc1924deeca434b8ac668f5d56b1aace2dbd82f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Aug 2015 23:26:54 +0200 Subject: [PATCH 02/12] Support the SYS_USE_IO=0 operation mode even with AUX mixer file present --- ROMFS/px4fmu_common/init.d/rc.interface | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 71b670888e..571e808b6f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -98,6 +98,13 @@ then fi # check if should load secondary mixer +set OUTPUT_AUX_DEV /dev/pwm_output1 +if [ -f $OUTPUT_AUX_DEV ] +then +else + set MIXER_AUX none +fi + if [ $MIXER_AUX != none ] then # @@ -105,7 +112,6 @@ then # set MIXER_AUX_FILE none - set OUTPUT_AUX_DEV /dev/pwm_output1 if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ] then @@ -169,3 +175,4 @@ then fi fi unset OUTPUT_DEV +unset OUTPUT_AUX_DEV From 809b6591c309d4a9941268f04d7368fd69fcd06d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 5 Aug 2015 16:16:03 +0200 Subject: [PATCH 03/12] Bring back the ability to transition plus a partial cleanup of tiltrotor support and firefly6 config updates. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 14 +++-- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 6 +- .../fw_att_control/fw_att_control_main.cpp | 6 +- src/modules/vtol_att_control/tiltrotor.cpp | 58 ++++++++----------- src/modules/vtol_att_control/tiltrotor.h | 3 +- .../vtol_att_control_main.cpp | 3 + 6 files changed, 42 insertions(+), 48 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 1ec65dff3e..52c6258c39 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_P 0.17 param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_FF 0.0 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.8 param set MC_YAW_FF 0.5 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + + param set VT_TILT_MC 0.08 + param set VT_TILT_TRANS 0.5 + param set VT_TILT_FW 0.9 fi set MIXER firefly6 diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 22dc2a69ce..8e31007ff1 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -5,18 +5,18 @@ Tilt mechanism servo mixer --------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 10000 10000 0 -10000 10000 +S: 1 4 0 20000 -10000 -10000 10000 Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 442aaa1702..ee78ef3110 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -800,7 +799,7 @@ FixedwingAttitudeControl::task_main() } /* if we are in rotary wing mode, do nothing */ - if (_vehicle_status.is_rotary_wing) { + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { continue; } @@ -813,11 +812,8 @@ FixedwingAttitudeControl::task_main() } /* decide if in stabilized or full manual control */ - if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale around tuning airspeed */ - float airspeed; /* if airspeed is not updating, we assume the normal average speed */ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2cf074fe42..e7ebab58e9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state() void Tiltrotor::process_mc_data() { - fill_mc_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_fw_state() @@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data() void Tiltrotor::process_fw_data() { - fill_fw_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_transition_state() { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition) { - _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + if (_params_tiltrotor.front_trans_dur <= 0.0f) { + _tilt_control = _params_tiltrotor.tilt_transition; + } else if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) { _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); } else { // at low speeds give full weight to mc @@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state() _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f); _roll_weight_mc = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { // tilt rotors forward up to certain angle - float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress; } _roll_weight_mc = progress; @@ -263,38 +267,26 @@ void Tiltrotor::update_external_state() } /** -* Prepare message to acutators with data from mc attitude controller. +* Prepare message to acutators with data from the attitude controllers. */ -void Tiltrotor::fill_mc_att_control_output() +void Tiltrotor::fill_att_control_output() { - _actuators_out_0->control[0] = _actuators_mc_in->control[0]; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2]; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle + } else { + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle + } _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon - + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control -} -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void Tiltrotor::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - /*controls for the elevons */ - _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon - _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle - _actuators_out_1->control[4] = _tilt_control; + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw } /** diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 3ef1362d00..07f3562027 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -99,8 +99,7 @@ private: float _tilt_control; float _roll_weight_mc; - void fill_mc_att_control_output(); - void fill_fw_att_control_output(); + void fill_att_control_output(); void set_max_mc(); void set_max_fw(unsigned pwm_value); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 589a08d8cd..7fbb43f769 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -531,6 +531,9 @@ void VtolAttitudeControl::task_main() // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); + + _vtol_type->process_mc_data(); + fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { From 9918ff9bd43493d59e4bc40b74adc624acc9eb26 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 00:12:14 +0200 Subject: [PATCH 04/12] Fix AUX ports, still retain successful boot for systems without IO --- ROMFS/px4fmu_common/init.d/rc.interface | 25 +++++++++++++------------ ROMFS/px4fmu_common/init.d/rcS | 14 +++++++------- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 571e808b6f..61abda3427 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -97,14 +97,6 @@ then fi fi -# check if should load secondary mixer -set OUTPUT_AUX_DEV /dev/pwm_output1 -if [ -f $OUTPUT_AUX_DEV ] -then -else - set MIXER_AUX none -fi - if [ $MIXER_AUX != none ] then # @@ -112,6 +104,7 @@ then # set MIXER_AUX_FILE none + set OUTPUT_AUX_DEV /dev/pwm_output1 if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ] then @@ -128,16 +121,24 @@ then then if fmu mode_pwm then - if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + if [ -f $OUTPUT_AUX_DEV ] then - echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + then + echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + else + echo "[i] Error loading mixer: $MIXER_AUX_FILE" + echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE + fi else - echo "[i] Error loading mixer: $MIXER_AUX_FILE" - echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE + set PWM_AUX_OUT none + set FAILSAFE_AUX none fi else echo "ERROR: Could not start: fmu mode_pwm" >> $LOG_FILE tone_alarm $TUNE_ERR + set PWM_AUX_OUT none + set FAILSAFE_AUX none fi if [ $PWM_AUX_OUT != none ] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 608ac95ef9..ad9a311ab4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -694,13 +694,6 @@ then fi unset FEXTRAS - if [ $EXIT_ON_END == yes ] - then - echo "Exit from nsh" - exit - fi - unset EXIT_ON_END - # Run no SD alarm if [ $LOG_FILE == /dev/null ] then @@ -712,6 +705,13 @@ then # Boot is complete, inform MAVLink app(s) that the system is now fully up and running mavlink boot_complete + if [ $EXIT_ON_END == yes ] + then + echo "Exit from nsh" + exit + fi + unset EXIT_ON_END + # End of autostart fi From e2c657ede0049d372b7171b696ba1ef64f309ba4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 00:13:09 +0200 Subject: [PATCH 05/12] MAVLink params: Eventually time out to make QGC happy, but let the user know that the boot failed. --- src/modules/mavlink/mavlink_parameters.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 66c6a87176..6903ebfc8c 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -215,6 +215,10 @@ MavlinkParametersManager::send(const hrt_abstime t) if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } + } else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) { + /* the boot did not seem to ever complete, warn user and set boot complete */ + _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); + _mavlink->set_boot_complete(); } } From 009f5282669407e5c7dda217e4276ef25dd2d7f5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 5 Aug 2015 18:34:46 -0700 Subject: [PATCH 06/12] Fixes for posix-arm and qurt builds Ifdefed out deadcode in position_estimator_inav_main.c as the deadcode does not compile for qurt. Added fixes to get a successful build for posix-arm and qurt targets. Removed CLOCK_REALTIME def from px4_time.h for qurt and removed unused variables in att_est_q and mc_att_control. Signed-off-by: Mark Charlebois --- makefiles/posix-arm/config_eagle_default.mk | 3 ++- makefiles/posix-arm/config_eagle_hil.mk | 1 + makefiles/posix-arm/config_eagle_muorb_test.mk | 1 + makefiles/qurt/config_qurt_default.mk | 2 +- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 1 - src/modules/mc_att_control/mc_att_control_main.cpp | 4 ---- src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- src/modules/simulator/simulator.h | 3 ++- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 1 + src/platforms/px4_time.h | 2 -- src/platforms/px4_workqueue.h | 4 ++++ src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 2 +- src/platforms/qurt/tests/muorb/muorb_test_example.cpp | 4 +++- 14 files changed, 20 insertions(+), 16 deletions(-) diff --git a/makefiles/posix-arm/config_eagle_default.mk b/makefiles/posix-arm/config_eagle_default.mk index 90afa155c0..406d0a4f7b 100644 --- a/makefiles/posix-arm/config_eagle_default.mk +++ b/makefiles/posix-arm/config_eagle_default.mk @@ -49,7 +49,7 @@ MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator MODULES += modules/commander -#MODULES += modules/controllib +MODULES += modules/controllib # # Libraries @@ -63,6 +63,7 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/work_queue # # Unit tests diff --git a/makefiles/posix-arm/config_eagle_hil.mk b/makefiles/posix-arm/config_eagle_hil.mk index c70ef198a8..9b8b5767b6 100644 --- a/makefiles/posix-arm/config_eagle_hil.mk +++ b/makefiles/posix-arm/config_eagle_hil.mk @@ -64,6 +64,7 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/work_queue #MODULES += platforms/posix/drivers/accelsim #MODULES += platforms/posix/drivers/gyrosim #MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/posix-arm/config_eagle_muorb_test.mk b/makefiles/posix-arm/config_eagle_muorb_test.mk index ec080c9195..e7547ef6e0 100644 --- a/makefiles/posix-arm/config_eagle_muorb_test.mk +++ b/makefiles/posix-arm/config_eagle_muorb_test.mk @@ -63,6 +63,7 @@ MODULES += modules/uORB # Linux port # MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/work_queue #MODULES += platforms/posix/drivers/accelsim #MODULES += platforms/posix/drivers/gyrosim #MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 5f154b110a..0891b04f2a 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -47,7 +47,7 @@ MODULES += modules/systemlib/mixer MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 -MODULES += modules/simulator +#MODULES += modules/simulator #MODULES += modules/commander # diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d9ee39de51..e91d69051e 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 1dcc830730..830f48dff8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -137,7 +137,6 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ int _motor_limits_sub; /**< motor limits subscription */ - orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub; /**< controller status publication */ @@ -170,8 +169,6 @@ private: math::Matrix<3, 3> _I; /**< identity matrix */ - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -315,7 +312,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _vehicle_status_sub(-1), /* publications */ - _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index d281c1a7a4..ffbf2ea049 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -457,7 +457,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data } if (topic_count_to_return != *topic_count) { - PX4_WARN("Not sending all topics: topics_to_return:[%d] topics_returning:[%d]", topic_count_to_return, *topic_count); + PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count); } _QueueMutex.unlock(); @@ -539,7 +539,7 @@ int32_t uORB::FastRpcChannel::copy_data_to_buffer(int32_t src_index, uint8_t *ds } else { PX4_WARN("Error coping the DataMsg to dst buffer, insuffienct space. "); - PX4_WARN("... offset[%d] len[%d] data_msg_len[%d]", + PX4_WARN("... offset[%ld] len[%ld] data_msg_len[%ld]", offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 158d2ef27d..e208e3bf04 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -46,8 +46,6 @@ #include #include #include -#include -#include #include #include #include @@ -192,6 +190,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) { return; +#ifndef __PX4_QURT FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { @@ -215,6 +214,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fsync(fileno(f)); fclose(f); +#endif } /**************************************************************************** diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a0901a9ad6..0009d9e1de 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -210,8 +210,9 @@ private: _baro_pub(nullptr), _gyro_pub(nullptr), _mag_pub(nullptr), - _initialized(false), + _initialized(false) #ifndef __PX4_QURT + , _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 5515ef9e9b..44f9ceff88 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -54,6 +54,7 @@ #include #include +#include #define MAX_CMD_LEN 100 diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index a54a18b9ab..5a93dedf40 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -12,8 +12,6 @@ #include -#define CLOCK_REALTIME 1 - __BEGIN_DECLS #if 0 diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index bb4b98b511..865d621a51 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -46,6 +46,10 @@ #include #include +#ifdef __PX4_QURT + #include +#endif + __BEGIN_DECLS #define HPWORK 0 diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 48c1e5f252..0e7fdc209a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -278,7 +278,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp index 7e93bf1149..2628b56a3b 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -96,7 +96,7 @@ int MuorbTestExample::DefaultTest() orb_publish(ORB_ID(pwm_input), pub_fd, &pwm); orb_publish(ORB_ID(sensor_combined), pub_sc, &sc); - sleep(1); + usleep(1000000); ++i; } @@ -194,6 +194,7 @@ int MuorbTestExample::FileReadTest() rc = PX4_ERROR; } else { + /* int i = 0; while( ( read = getline( &line, &len, fp ) ) != -1 ) { @@ -201,6 +202,7 @@ int MuorbTestExample::FileReadTest() PX4_INFO( "LineNum[%d] LineLength[%d]", i, len ); PX4_INFO( "LineNum[%d] Line[%s]", i, line ); } + */ PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH); fclose(fp); From 003a3b0b3663b03b07bdb2f6771b80b2a190a7d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 09:38:15 +0200 Subject: [PATCH 07/12] Fix file presence operator (thanks to @sjwilks) --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 61abda3427..f3761200a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -121,7 +121,7 @@ then then if fmu mode_pwm then - if [ -f $OUTPUT_AUX_DEV ] + if [ -e $OUTPUT_AUX_DEV ] then if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE then From 5adb3cc308aebeda3dbf45ca7625ecf384bb2072 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 5 Aug 2015 18:03:44 +0200 Subject: [PATCH 08/12] Add support for a 'standard' VTOL with pusher/tractor motor. --- .../px4fmu_common/init.d/13005_vtol_AERT_quad | 50 +++ ROMFS/px4fmu_common/init.d/rcS | 4 + ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix | 30 ++ .../px4fmu_common/mixers/vtol_quad_x.main.mix | 4 + src/modules/vtol_att_control/module.mk | 4 +- src/modules/vtol_att_control/standard.cpp | 296 ++++++++++++++++++ src/modules/vtol_att_control/standard.h | 107 +++++++ .../vtol_att_control/standard_params.c | 51 +++ .../vtol_att_control/tiltrotor_params.c | 39 +-- .../vtol_att_control_main.cpp | 3 + .../vtol_att_control/vtol_att_control_main.h | 2 + .../vtol_att_control_params.c | 48 ++- src/modules/vtol_att_control/vtol_type.h | 4 +- 13 files changed, 601 insertions(+), 41 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix create mode 100644 src/modules/vtol_att_control/standard.cpp create mode 100644 src/modules/vtol_att_control/standard.h create mode 100644 src/modules/vtol_att_control/standard_params.c diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad new file mode 100644 index 0000000000..a3e183a84a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad @@ -0,0 +1,50 @@ +#!nsh +# +# @name Generic AERT with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Simon Wilks +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6e74704e1a..098c23a9d6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -637,6 +637,10 @@ then then set MAV_TYPE 21 fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi fi # Still no MAV_TYPE found diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix new file mode 100644 index 0000000000..260e60840a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix @@ -0,0 +1,30 @@ +Mixer for a horizontal format with X-Quad and tractor AERT plane configuration +============================================================================== + +Motor speed mixer +----------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + +Aileron mixers +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Elevator mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix new file mode 100644 index 0000000000..92bfce6c8c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix @@ -0,0 +1,4 @@ +# VTOL quad X mixer for PX4FMU +#============================= + +R: 4x 10000 10000 10000 0 diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index ad6efd2b27..d3f9326b04 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \ tiltrotor_params.c \ tiltrotor.cpp \ vtol_type.cpp \ - tailsitter.cpp + tailsitter.cpp \ + standard_params.c \ + standard.cpp EXTRACXXFLAGS = -Wno-write-strings diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp new file mode 100644 index 0000000000..c38a935ff7 --- /dev/null +++ b/src/modules/vtol_att_control/standard.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file standard.cpp + * + * @author Simon Wilks + * @author Roman Bapst + * +*/ + +#include "standard.h" +#include "vtol_att_control_main.h" + +Standard::Standard(VtolAttitudeControl *attc) : + VtolType(attc), + _flag_enable_mc_motors(true), + _pusher_throttle(0.0f), + _mc_att_ctl_weight(1.0f), + _airspeed_trans_blend_margin(0.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); + _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); + } + +Standard::~Standard() +{ +} + +int +Standard::parameters_update() +{ + float v; + + /* duration of a forwards transition to fw mode */ + param_get(_params_handles_standard.front_trans_dur, &v); + _params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* duration of a back transition to mc mode */ + param_get(_params_handles_standard.back_trans_dur, &v); + _params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* target throttle value for pusher motor during the transition to fw mode */ + param_get(_params_handles_standard.pusher_trans, &v); + _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); + + /* airspeed at which it we should switch to fw mode */ + param_get(_params_handles_standard.airspeed_trans, &v); + _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); + + /* airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_standard.airspeed_blend, &v); + _params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f); + + _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; + + return OK; +} + +void Standard::update_vtol_state() +{ + parameters_update(); + + /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * For the back transition the pusher motor is immediately stopped and rotors reactivated. + */ + + if (_manual_control_sp->aux1 < 0.0f) { + // the transition to fw mode switch is off + if (_vtol_schedule.flight_mode == MC_MODE) { + // in mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // transition to mc mode + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // failsafe back to mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // keep transitioning to mc mode + _vtol_schedule.flight_mode = MC_MODE; + } + + // the pusher motor should never be powered when in or transitioning to mc mode + _pusher_throttle = 0.0f; + + } else { + // the transition to fw mode switch is on + if (_vtol_schedule.flight_mode == MC_MODE) { + // start transition to fw mode + _vtol_schedule.flight_mode = TRANSITION_TO_FW; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // in fw mode + _vtol_schedule.flight_mode = FW_MODE; + _mc_att_ctl_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + _vtol_schedule.flight_mode = FW_MODE; + // we can turn off the multirotor motors now + _flag_enable_mc_motors = false; + // don't set pusher throttle here as it's being ramped up elsewhere + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // transitioning to mc mode & transition switch on - failsafe back into fw mode + _vtol_schedule.flight_mode = FW_MODE; + } + } + + // map specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; + } +} + +void Standard::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_params_standard.front_trans_dur <= 0.0f) { + // just set the final target throttle value + _pusher_throttle = _params_standard.pusher_trans; + } else if (_pusher_throttle <= _params_standard.pusher_trans) { + // ramp up throttle to the target throttle value + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + } + + // do blending of mc and fw controls if a blending airspeed has been provided + if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { + _mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + } else { + // at low speeds give full weight to mc + _mc_att_ctl_weight = 1.0f; + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // continually increase mc attitude control as we transition back to mc mode + if (_params_standard.back_trans_dur > 0.0f) { + _mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + } else { + _mc_att_ctl_weight = 1.0f; + } + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (_flag_enable_mc_motors) { + set_max_mc(2000); + set_idle_mc(); + _flag_enable_mc_motors = false; + } + } + + _mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f); +} + +void Standard::update_mc_state() +{ + // do nothing +} + +void Standard::process_mc_data() +{ + fill_att_control_output(); +} + +void Standard::process_fw_data() +{ + fill_att_control_output(); +} + + void Standard::update_fw_state() +{ + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (!_flag_enable_mc_motors) { + set_max_mc(950); + set_idle_fw(); // force them to stop, not just idle + _flag_enable_mc_motors = true; + } + } + +void Standard::update_external_state() +{ +} + +/** + * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine + * what proportion of control should be applied to each of the control groups (mc and fw). + */ +void Standard::fill_att_control_output() +{ + /* multirotor controls */ + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle + + /* fixed wing controls */ + const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight; + _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch + _actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw + + // set the fixed wing throttle control + if (_vtol_schedule.flight_mode == FW_MODE) { + // take the throttle value commanded by the fw controller + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; + } else { + // otherwise we may be ramping up the throttle during the transition to fw mode + _actuators_out_1->control[3] = _pusher_throttle; + } +} + +/** +* Disable all multirotor motors when in fw mode. +*/ +void +Standard::set_max_mc(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h new file mode 100644 index 0000000000..e4e1d92d85 --- /dev/null +++ b/src/modules/vtol_att_control/standard.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file standard.h + * VTOL with fixed multirotor motor configurations (such as quad) and a pusher + * (or puller aka tractor) motor for forward flight. + * + * @author Simon Wilks + * @author Roman Bapst + * + */ + +#ifndef STANDARD_H +#define STANDARD_H +#include "vtol_type.h" +#include +#include + +class Standard : public VtolType +{ + +public: + + Standard(VtolAttitudeControl * _att_controller); + ~Standard(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float pusher_trans; + float airspeed_blend; + float airspeed_trans; + } _params_standard; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t pusher_trans; + param_t airspeed_blend; + param_t airspeed_trans; + } _params_handles_standard; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_TO_FW, + TRANSITION_TO_MC, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + }_vtol_schedule; + + bool _flag_enable_mc_motors; + float _pusher_throttle; + float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning + float _airspeed_trans_blend_margin; + + void fill_att_control_output(); + void set_max_mc(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c new file mode 100644 index 0000000000..134dcd0b8a --- /dev/null +++ b/src/modules/vtol_att_control/standard_params.c @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file standard_params.c + * Parameters for the standard VTOL controller. + * + * @author Simon Wilks + * @author Roman Bapst + */ + +#include + +/** + * Target throttle value for pusher/puller motor during the transition to fw mode + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f); diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 7d233f6f50..d4e925f072 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -39,28 +39,6 @@ */ #include - -/** - * Duration of a front transition - * - * Time in seconds used for a transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); - -/** - * Duration of a back transition - * - * Time in seconds used for a back transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); /** * Position of tilt servo in mc mode @@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); /** * Position of tilt servo in transition mode @@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); /** * Position of tilt servo in fw mode @@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); - -/** - * Transition airspeed - * - * Airspeed at which we can switch to fw mode - * - * @min 0.0 - * @max 20 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 7fbb43f769..92a591de51 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -123,6 +123,9 @@ VtolAttitudeControl::VtolAttitudeControl() : } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { + _standard = new Standard(this); + _vtol_type = _standard; } else { _task_should_exit = true; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 43e8969929..bf9a69d5c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include "tiltrotor.h" #include "tailsitter.h" +#include "standard.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); @@ -188,6 +189,7 @@ private: VtolType * _vtol_type; // base class for different vtol types Tiltrotor * _tiltrotor; // tailsitter vtol type Tailsitter * _tailsitter; // tiltrotor vtol type + Standard * _standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 33c095036c..56779ca8f9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** - * VTOL Type (Tailsitter=0, Tiltrotor=1) + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * * @min 0 - * @max 1 + * @max 2 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); @@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 1.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index bbe6a8642e..a7fdbdde1a 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -38,8 +38,8 @@ * */ -#ifndef VTOL_YYPE_H -#define VTOL_YYPE_H +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H struct Params { int idle_pwm_mc; // pwm value for idle in mc mode From 333ba769eff38872e2d2b0d0c6fff8e5e6b3cdb8 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 6 Aug 2015 08:23:39 +0200 Subject: [PATCH 09/12] Don't use the control surface abbreviation but use the actual channel assignment. --- ...5_vtol_AERT_quad => 13005_vtol_AAERT_quad} | 4 ++-- .../{vtol_AERT.aux.mix => vtol_AAERT.aux.mix} | 22 ++++++++++--------- 2 files changed, 14 insertions(+), 12 deletions(-) rename ROMFS/px4fmu_common/init.d/{13005_vtol_AERT_quad => 13005_vtol_AAERT_quad} (91%) rename ROMFS/px4fmu_common/mixers/{vtol_AERT.aux.mix => vtol_AAERT.aux.mix} (71%) diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad similarity index 91% rename from ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad rename to ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad index a3e183a84a..4b8191f557 100644 --- a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad @@ -1,6 +1,6 @@ #!nsh # -# @name Generic AERT with Quad VTOL. +# @name Generic AAERT tailplane airframe with Quad VTOL. # # @type Standard VTOL # @@ -36,7 +36,7 @@ fi set MIXER vtol_quad_x set PWM_OUT 12345678 -set MIXER_AUX vtol_AERT +set MIXER_AUX vtol_AAERT set PWM_AUX_RATE 50 set PWM_AUX_OUT 1234 set PWM_AUX_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix similarity index 71% rename from ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix rename to ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix index 260e60840a..f3e9c42354 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix @@ -1,18 +1,14 @@ -Mixer for a horizontal format with X-Quad and tractor AERT plane configuration -============================================================================== +Mixer for an AAERT VTOL +======================= -Motor speed mixer ------------------ -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 3 0 20000 -10000 -10000 10000 - -Aileron mixers --------------- +Aileron 1 mixer +--------------- M: 1 O: 10000 10000 0 -10000 10000 S: 1 0 -7500 -7500 0 -10000 10000 +Aileron 2 mixer +--------------- M: 1 O: 10000 10000 0 -10000 10000 S: 1 0 -7500 -7500 0 -10000 10000 @@ -28,3 +24,9 @@ Rudder mixer M: 1 O: 10000 10000 0 -10000 10000 S: 1 2 -10000 -10000 0 -10000 10000 + +Throttle mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 From 7e782fe92a098645cf1bbfe2404b263f3a061d08 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 6 Aug 2015 15:24:52 +0200 Subject: [PATCH 10/12] adding landed detector for fixed wing so alt/posctl works --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 265364fac8..7fe494fad1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -13,3 +13,9 @@ mc_att_control start mc_pos_control start fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# Fixed wing for now until we have something for VTOL +# +land_detector start fixedwing From 1dbef49a54620c983921843277dcb651daf9ac65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 20:48:53 +0200 Subject: [PATCH 11/12] Fix TBS disco default gains --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 2058d10091..fabe12703f 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -12,12 +12,12 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then # TODO review MC_YAWRATE_I - param set MC_ROLL_P 8.0 - param set MC_ROLLRATE_P 0.07 + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.1 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.0017 - param set MC_PITCH_P 8.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.1 param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 From faae76e3c798e2dc3d4e920e29f1f46c2bf27d98 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 6 Aug 2015 12:51:20 -0700 Subject: [PATCH 12/12] Removed __PX4_QURT ifdefs from application layer Replaced test for __PX4_QURT with test for NAV_DEBUG. Optimized implementation to remove code when flag is not set. Signed-off-by: Mark Charlebois --- .../position_estimator_inav/position_estimator_inav_main.c | 7 ++++--- src/platforms/posix/include/px4_platform_types.h | 3 +++ src/platforms/px4_workqueue.h | 7 ++----- 3 files changed, 9 insertions(+), 8 deletions(-) create mode 100644 src/platforms/posix/include/px4_platform_types.h diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index e208e3bf04..05687d0943 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -185,12 +185,11 @@ int position_estimator_inav_main(int argc, char *argv[]) return 1; } +#ifdef INAV_DEBUG static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) { - return; -#ifndef __PX4_QURT FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { @@ -214,8 +213,10 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fsync(fileno(f)); fclose(f); -#endif } +#else +#define write_debug_log(...) +#endif /**************************************************************************** * main diff --git a/src/platforms/posix/include/px4_platform_types.h b/src/platforms/posix/include/px4_platform_types.h new file mode 100644 index 0000000000..abaf177444 --- /dev/null +++ b/src/platforms/posix/include/px4_platform_types.h @@ -0,0 +1,3 @@ +#ifdef __PX4_QURT +#include +#endif diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 865d621a51..79162e3f6b 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -41,14 +41,11 @@ #include #include #include -#elif defined(__PX4_POSIX) || defined(__PX4_QURT) +#elif defined(__PX4_POSIX) #include #include - -#ifdef __PX4_QURT - #include -#endif +#include __BEGIN_DECLS