Browse Source

Merge remote-tracking branch 'remotes/px4/master' into px4fix/masterMultiMavlinkFix

sbg
oberion 10 years ago
parent
commit
6fa01aa37f
  1. 8
      ROMFS/px4fmu_common/init.d/10015_tbs_discovery
  2. 14
      ROMFS/px4fmu_common/init.d/13002_firefly6
  3. 50
      ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad
  4. 25
      ROMFS/px4fmu_common/init.d/rc.interface
  5. 6
      ROMFS/px4fmu_common/init.d/rc.vtol_apps
  6. 18
      ROMFS/px4fmu_common/init.d/rcS
  7. 6
      ROMFS/px4fmu_common/mixers/firefly6.aux.mix
  8. 32
      ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix
  9. 4
      ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix
  10. 3
      makefiles/posix-arm/config_eagle_default.mk
  11. 1
      makefiles/posix-arm/config_eagle_hil.mk
  12. 1
      makefiles/posix-arm/config_eagle_muorb_test.mk
  13. 2
      makefiles/qurt/config_qurt_default.mk
  14. 1
      src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
  15. 6
      src/modules/fw_att_control/fw_att_control_main.cpp
  16. 4
      src/modules/mavlink/mavlink_parameters.cpp
  17. 4
      src/modules/mc_att_control/mc_att_control_main.cpp
  18. 4
      src/modules/muorb/adsp/uORBFastRpcChannel.cpp
  19. 7
      src/modules/position_estimator_inav/position_estimator_inav_main.c
  20. 3
      src/modules/simulator/simulator.h
  21. 4
      src/modules/vtol_att_control/module.mk
  22. 296
      src/modules/vtol_att_control/standard.cpp
  23. 107
      src/modules/vtol_att_control/standard.h
  24. 51
      src/modules/vtol_att_control/standard_params.c
  25. 58
      src/modules/vtol_att_control/tiltrotor.cpp
  26. 3
      src/modules/vtol_att_control/tiltrotor.h
  27. 39
      src/modules/vtol_att_control/tiltrotor_params.c
  28. 6
      src/modules/vtol_att_control/vtol_att_control_main.cpp
  29. 2
      src/modules/vtol_att_control/vtol_att_control_main.h
  30. 48
      src/modules/vtol_att_control/vtol_att_control_params.c
  31. 4
      src/modules/vtol_att_control/vtol_type.h
  32. 3
      src/platforms/posix/include/px4_platform_types.h
  33. 1
      src/platforms/posix/px4_layer/px4_posix_tasks.cpp
  34. 2
      src/platforms/px4_time.h
  35. 3
      src/platforms/px4_workqueue.h
  36. 2
      src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
  37. 4
      src/platforms/qurt/tests/muorb/muorb_test_example.cpp

8
ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@ -12,12 +12,12 @@ sh /etc/init.d/rc.mc_defaults @@ -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

14
ROMFS/px4fmu_common/init.d/13002_firefly6

@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults @@ -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

50
ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad

@ -0,0 +1,50 @@ @@ -0,0 +1,50 @@
#!nsh
#
# @name Generic AAERT tailplane airframe with Quad VTOL.
#
# @type Standard VTOL
#
# @maintainer Simon Wilks <simon@uaventure.com>
#
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_AAERT
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

25
ROMFS/px4fmu_common/init.d/rc.interface

@ -97,14 +97,6 @@ then @@ -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 @@ -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 @@ -128,16 +121,24 @@ then
then
if fmu mode_pwm
then
if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
if [ -e $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 ]

6
ROMFS/px4fmu_common/init.d/rc.vtol_apps

@ -13,3 +13,9 @@ mc_att_control start @@ -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

18
ROMFS/px4fmu_common/init.d/rcS

@ -637,6 +637,10 @@ then @@ -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
@ -705,13 +709,6 @@ then @@ -705,13 +709,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
@ -723,6 +720,13 @@ then @@ -723,6 +720,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

6
ROMFS/px4fmu_common/mixers/firefly6.aux.mix

@ -5,18 +5,18 @@ Tilt mechanism servo mixer @@ -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

32
ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix

@ -0,0 +1,32 @@ @@ -0,0 +1,32 @@
Mixer for an AAERT VTOL
=======================
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
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
Throttle mixer
--------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 3 0 20000 -10000 -10000 10000

4
ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix

@ -0,0 +1,4 @@ @@ -0,0 +1,4 @@
# VTOL quad X mixer for PX4FMU
#=============================
R: 4x 10000 10000 10000 0

3
makefiles/posix-arm/config_eagle_default.mk

@ -49,7 +49,7 @@ MODULES += modules/dataman @@ -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 @@ -63,6 +63,7 @@ MODULES += lib/conversion
# Linux port
#
MODULES += platforms/posix/px4_layer
MODULES += platforms/posix/work_queue
#
# Unit tests

1
makefiles/posix-arm/config_eagle_hil.mk

@ -64,6 +64,7 @@ MODULES += lib/conversion @@ -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

1
makefiles/posix-arm/config_eagle_muorb_test.mk

@ -63,6 +63,7 @@ MODULES += modules/uORB @@ -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

2
makefiles/qurt/config_qurt_default.mk

@ -47,7 +47,7 @@ MODULES += modules/systemlib/mixer @@ -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
#

1
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@ -47,7 +47,6 @@ @@ -47,7 +47,6 @@
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>

6
src/modules/fw_att_control/fw_att_control_main.cpp

@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main() @@ -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() @@ -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() @@ -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 */

4
src/modules/mavlink/mavlink_parameters.cpp

@ -215,6 +215,10 @@ MavlinkParametersManager::send(const hrt_abstime t) @@ -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();
}
}

4
src/modules/mc_att_control/mc_att_control_main.cpp

@ -137,7 +137,6 @@ private: @@ -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: @@ -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() : @@ -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),

4
src/modules/muorb/adsp/uORBFastRpcChannel.cpp

@ -457,7 +457,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data @@ -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 @@ -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);
}

7
src/modules/position_estimator_inav/position_estimator_inav_main.c

@ -46,8 +46,6 @@ @@ -46,8 +46,6 @@
#include <fcntl.h>
#include <string.h>
#include <px4_config.h>
#include <sys/prctl.h>
#include <termios.h>
#include <math.h>
#include <float.h>
#include <uORB/uORB.h>
@ -187,11 +185,11 @@ int position_estimator_inav_main(int argc, char *argv[]) @@ -187,11 +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;
FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a");
if (f) {
@ -216,6 +214,9 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e @@ -216,6 +214,9 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
fsync(fileno(f));
fclose(f);
}
#else
#define write_debug_log(...)
#endif
/****************************************************************************
* main

3
src/modules/simulator/simulator.h

@ -210,8 +210,9 @@ private: @@ -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),

4
src/modules/vtol_att_control/module.mk

@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \ @@ -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

296
src/modules/vtol_att_control/standard.cpp

@ -0,0 +1,296 @@ @@ -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 <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#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);
}

107
src/modules/vtol_att_control/standard.h

@ -0,0 +1,107 @@ @@ -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 <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef STANDARD_H
#define STANDARD_H
#include "vtol_type.h"
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
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

51
src/modules/vtol_att_control/standard_params.c

@ -0,0 +1,51 @@ @@ -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 <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* 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);

58
src/modules/vtol_att_control/tiltrotor.cpp

@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state() @@ -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() @@ -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() @@ -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() @@ -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
}
/**

3
src/modules/vtol_att_control/tiltrotor.h

@ -99,8 +99,7 @@ private: @@ -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);

39
src/modules/vtol_att_control/tiltrotor_params.c

@ -39,28 +39,6 @@ @@ -39,28 +39,6 @@
*/
#include <systemlib/param/param.h>
/**
* 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); @@ -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); @@ -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); @@ -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);

6
src/modules/vtol_att_control/vtol_att_control_main.cpp

@ -123,6 +123,9 @@ VtolAttitudeControl::VtolAttitudeControl() : @@ -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;
}
@ -531,6 +534,9 @@ void VtolAttitudeControl::task_main() @@ -531,6 +534,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) {

2
src/modules/vtol_att_control/vtol_att_control_main.h

@ -84,6 +84,7 @@ @@ -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: @@ -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***********************************************************************

48
src/modules/vtol_att_control/vtol_att_control_params.c

@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); @@ -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); @@ -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);

4
src/modules/vtol_att_control/vtol_type.h

@ -38,8 +38,8 @@ @@ -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

3
src/platforms/posix/include/px4_platform_types.h

@ -0,0 +1,3 @@ @@ -0,0 +1,3 @@
#ifdef __PX4_QURT
#include <dspal_types.h>
#endif

1
src/platforms/posix/px4_layer/px4_posix_tasks.cpp

@ -54,6 +54,7 @@ @@ -54,6 +54,7 @@
#include <string>
#include <px4_tasks.h>
#include <px4_posix.h>
#define MAX_CMD_LEN 100

2
src/platforms/px4_time.h

@ -12,8 +12,6 @@ @@ -12,8 +12,6 @@
#include <sys/timespec.h>
#define CLOCK_REALTIME 1
__BEGIN_DECLS
#if 0

3
src/platforms/px4_workqueue.h

@ -41,10 +41,11 @@ @@ -41,10 +41,11 @@
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#elif defined(__PX4_POSIX) || defined(__PX4_QURT)
#elif defined(__PX4_POSIX)
#include <stdint.h>
#include <queue.h>
#include <px4_platform_types.h>
__BEGIN_DECLS

2
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@ -278,7 +278,7 @@ void px4_show_tasks() @@ -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++;
}
}

4
src/platforms/qurt/tests/muorb/muorb_test_example.cpp

@ -96,7 +96,7 @@ int MuorbTestExample::DefaultTest() @@ -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() @@ -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() @@ -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);

Loading…
Cancel
Save