This commit is contained in:
ChristophTobler
2015-11-10 16:19:17 +01:00
92 changed files with 1268 additions and 1394 deletions
+3
View File
@@ -25,3 +25,6 @@
[submodule "unittests/googletest"]
path = unittests/googletest
url = https://github.com/google/googletest.git
[submodule "src/lib/matrix"]
path = src/lib/matrix
url = https://github.com/PX4/Matrix.git
+1 -1
View File
@@ -118,7 +118,7 @@ after_success:
cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4
&& cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4
&& zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
&& ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
&& ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
&& ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
+1
View File
@@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
add_custom_target(submodule_clean
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+11
View File
@@ -4,6 +4,17 @@
#
# @type Hexarotor Coaxial
#
# @output MAIN1 front right top, CW; angle:60; direction:CW
# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW
# @output MAIN3 back top, CW; angle:180; direction:CW
# @output MAIN4 back bottom, CCW; angle:180; direction:CCW
# @output MAIN5 front left top, CW; angle:-60; direction:CW
# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Lorenz Meier <lorenz@px4.io>
#
+4 -1
View File
@@ -14,7 +14,7 @@
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Simon Wilks <simon@px4.io>
# @maintainer Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.fw_defaults
@@ -39,6 +39,9 @@ then
param set FW_RR_P 0.04
fi
# Configure this as plane
set MAV_TYPE 1
# Set mixer
set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUT 4
+33
View File
@@ -0,0 +1,33 @@
#!nsh
#
# @name Lumenier QAV250
#
# @type Quadrotor x
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Mark Whitehorn <kd0aij@gmail.com>
#
sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.002
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.1
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set PWM_MIN 980
param set MPC_THR_MIN 0.06
param set MPC_MANTHR_MIN 0.06
fi
+146
View File
@@ -0,0 +1,146 @@
<?xml version='1.0' encoding='UTF-8'?>
<parameters>
<version>3</version>
<group name="UAVCAN Motor Parameters" no_code_generation="true">
<parameter default="75" name="ctl_bw" type="INT32">
<short_desc>Speed controller bandwidth</short_desc>
<long_desc>Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness.</long_desc>
<unit>Hertz</unit>
<min>10</min>
<max>250</max>
</parameter>
<parameter default="1" name="ctl_dir" type="INT32">
<short_desc>Reverse direction</short_desc>
<long_desc>Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction.</long_desc>
<min>0</min>
<max>1</max>
</parameter>
<parameter default="1" name="ctl_gain" type="FLOAT">
<short_desc>Speed (RPM) controller gain</short_desc>
<long_desc>Speed (RPM) controller gain. Determines controller
aggressiveness; units are amp-seconds per radian. Systems with
higher rotational inertia (large props) will need gain increased;
systems with low rotational inertia (small props) may need gain
decreased. Higher values result in faster response, but may result
in oscillation and excessive overshoot. Lower values result in a
slower, smoother response.</long_desc>
<unit>amp-seconds per radian</unit>
<decimal>3</decimal>
<min>0.00</min>
<max>1.00</max>
</parameter>
<parameter default="3.5" name="ctl_hz_idle" type="FLOAT">
<short_desc>Idle speed (e Hz)</short_desc>
<long_desc>Idle speed (e Hz)</long_desc>
<unit>Hertz</unit>
<decimal>3</decimal>
<min>0.0</min>
<max>100.0</max>
</parameter>
<parameter default="25" name="ctl_start_rate" type="INT32">
<short_desc>Spin-up rate (e Hz/s)</short_desc>
<long_desc>Spin-up rate (e Hz/s)</long_desc>
<unit>Hz/s</unit>
<decimal></decimal>
<min>5</min>
<max>1000</max>
</parameter>
<parameter default="0" name="esc_index" type="INT32">
<short_desc>Index of this ESC in throttle command messages.</short_desc>
<long_desc>Index of this ESC in throttle command messages.</long_desc>
<unit>Index</unit>
<decimal></decimal>
<min>0</min>
<max>15</max>
</parameter>
<parameter default="20034" name="id_ext_status" type="INT32">
<short_desc>Extended status ID</short_desc>
<long_desc>Extended status ID</long_desc>
<unit></unit>
<decimal></decimal>
<min>1</min>
<max>1023</max>
</parameter>
<parameter default="50000" name="int_ext_status" type="INT32">
<short_desc>Extended status interval (µs)</short_desc>
<long_desc>Extended status interval (µs)</long_desc>
<unit>µs</unit>
<decimal></decimal>
<min>0</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_status" type="INT32">
<short_desc>ESC status interval (µs)</short_desc>
<long_desc>ESC status interval (µs)</long_desc>
<unit>µs</unit>
<decimal></decimal>
<max>1000000</max>
</parameter>
<parameter default="12" name="mot_i_max" type="FLOAT">
<short_desc>Motor current limit in amps</short_desc>
<long_desc>Motor current limit in amps. This determines the maximum
current controller setpoint, as well as the maximum allowable
current setpoint slew rate. This value should generally be set to
the continuous current rating listed in the motor’s specification
sheet, or set equal to the motor’s specified continuous power
divided by the motor voltage limit.</long_desc>
<unit>Amps</unit>
<decimal>3</decimal>
<min>1</min>
<max>80</max>
</parameter>
<parameter default="2300" name="mot_kv" type="INT32">
<short_desc>Motor Kv in RPM per volt</short_desc>
<long_desc>Motor Kv in RPM per volt. This can be taken from the motor’s
specification sheet; accuracy will help control performance but
some deviation from the specified value is acceptable.</long_desc>
<unit>RPM/v</unit>
<decimal>0</decimal>
<min>0</min>
<max>100</max>
</parameter>
<parameter default="0.0" name="mot_ls" type="FLOAT">
<short_desc>READ ONLY: Motor inductance in henries.</short_desc>
<long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
<unit>henries</unit>
<decimal>3</decimal>
</parameter>
<parameter default="14" name="mot_num_poles" type="INT32">
<short_desc>Number of motor poles.</short_desc>
<long_desc>Number of motor poles. Used to convert mechanical speeds to
electrical speeds. This number should be taken from the motor’s
specification sheet.</long_desc>
<unit>Poles</unit>
<decimal></decimal>
<min>2</min>
<max>40</max>
</parameter>
<parameter default="0.0" name="mot_rs" type="FLOAT">
<short_desc>READ ONLY: Motor resistance in ohms</short_desc>
<long_desc>READ ONLY: Motor resistance in ohms. This is measured on start-up. When
tuning a new motor, check that this value is approximately equal
to the value shown in the motor’s specification sheet.</long_desc>
<unit>Ohms</unit>
<decimal>3</decimal>
</parameter>
<parameter default=".5" name="mot_v_accel" type="INT32">
<short_desc>Acceleration limit (V)</short_desc>
<long_desc>Acceleration limit (V)</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0.01</min>
<max>1.00</max>
</parameter>
<parameter default="14.8" name="mot_v_max" type="FLOAT">
<short_desc>Motor voltage limit in volts</short_desc>
<long_desc>Motor voltage limit in volts. The current controller’s
commanded voltage will never exceed this value. Note that this may
safely be above the nominal voltage of the motor; to determine the
actual motor voltage limit, divide the motor’s rated power by the
motor current limit.</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0</min>
</parameter>
</group>
</parameters>
+5 -1
View File
@@ -72,9 +72,13 @@ class XMLOutput():
xml_field.text = value
for code in param.GetOutputCodes():
value = param.GetOutputValue(code)
valstrs = value.split(";")
xml_field = ET.SubElement(xml_param, "output")
xml_field.attrib["name"] = code
xml_field.text = value
for attrib in valstrs[1:]:
attribstrs = attrib.split(":")
xml_field.attrib[attribstrs[0].strip()] = attribstrs[1].strip()
xml_field.text = valstrs[0]
if last_param_name != param.GetName():
board_specific_param_set = False
indent(xml_parameters)
+5 -1
View File
@@ -18,10 +18,14 @@ def indent(elem, level=0):
class XMLOutput():
def __init__(self, groups, board):
def __init__(self, groups, board, inject_xml_file_name):
xml_parameters = ET.Element("parameters")
xml_version = ET.SubElement(xml_parameters, "version")
xml_version.text = "3"
importtree = ET.parse(inject_xml_file_name)
injectgroups = importtree.getroot().findall("group")
for igroup in injectgroups:
xml_parameters.append(igroup)
last_param_name = ""
board_specific_param_set = False
for group in groups:
+3 -2
View File
@@ -29,7 +29,7 @@ start_name = ""
end_name = ""
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
header += """
/*****************************************************************
* %s
@@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = {
"""
i=0
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
src += """
/*****************************************************************
* %s
+7 -1
View File
@@ -65,6 +65,12 @@ def main():
metavar="FILENAME",
help="Create XML file"
" (default FILENAME: parameters.xml)")
parser.add_argument("-i", "--inject-xml",
nargs='?',
const="../Tools/parameters_injected.xml",
metavar="FILENAME",
help="Inject additional param XML file"
" (default FILENAME: ../Tools/parameters_injected.xml)")
parser.add_argument("-b", "--board",
nargs='?',
const="",
@@ -124,7 +130,7 @@ def main():
# Output to XML file
if args.xml:
print("Creating XML file " + args.xml)
out = xmlout.XMLOutput(param_groups, args.board)
out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml)
out.Save(args.xml)
# Output to DokuWiki tables
+3 -2
View File
@@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image
"""
from __future__ import print_function
import argparse
import argparse, re
import os
@@ -64,7 +64,7 @@ def main():
# read file line by line
pruned_content = ""
with open(file_path, "r") as f:
with open(file_path, "rU") as f:
for line in f:
# handle mixer files differently than startup files
@@ -76,6 +76,7 @@ def main():
pruned_content += line
# overwrite old scratch file
with open(file_path, "wb") as f:
pruned_content = re.sub("\r\n", "\n", pruned_content)
f.write(pruned_content.encode("ascii", errors='strict'))
+6
View File
@@ -83,6 +83,12 @@ then
elif [ "$debugger" == "gdb" ]
then
gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "ddd" ]
then
ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "valgrind" ]
then
valgrind ./mainapp ../../../../${rc_script}_${program}
else
./mainapp ../../../../${rc_script}_${program}
fi
+2 -2
View File
@@ -622,7 +622,7 @@ function(px4_add_common_flags)
)
list(APPEND added_include_dirs
src/lib/eigen
src/lib/matrix
)
set(added_link_dirs) # none used currently
@@ -741,7 +741,7 @@ function(px4_generate_parameters_xml)
)
add_custom_command(OUTPUT ${OUT}
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
-s ${path} --board CONFIG_ARCH_${BOARD} --xml
-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
DEPENDS ${param_src_files}
)
set(${OUT} ${${OUT}} PARENT_SCOPE)
+1
View File
@@ -36,6 +36,7 @@ set(config_module_list
modules/ekf_att_pos_estimator
modules/position_estimator_inav
modules/navigator
modules/vtol_att_control
modules/mc_pos_control
modules/mc_att_control
modules/mc_pos_control_multiplatform
+2
View File
@@ -81,6 +81,7 @@ add_message_files(
offboard_control_mode.msg
vehicle_force_setpoint.msg
distance_sensor.msg
control_state.msg
)
## Generate services in the 'srv' folder
@@ -135,6 +136,7 @@ include_directories(
${CMAKE_BINARY_DIR}/src/modules
src/
src/lib
src/lib/matrix
${EIGEN_INCLUDE_DIRS}
integrationtests
)
+18
View File
@@ -0,0 +1,18 @@
# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
uint64 timestamp # in microseconds since system start
float32 x_acc # X acceleration in body frame
float32 y_acc # Y acceleration in body frame
float32 z_acc # Z acceleration in body frame
float32 x_vel # X velocity in body frame
float32 y_vel # Y velocity in body frame
float32 z_vel # Z velocity in body frame
float32 x_pos # X position in local frame
float32 y_pos # Y position in local frame
float32 z_pos # z position in local frame
float32 airspeed # Airspeed, estimated
float32[3] vel_variance # Variance in body velocity estimate
float32[3] pos_variance # Variance in local position estimate
float32[4] q # Attitude Quaternion
float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down)
float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down)
float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down)
+1
View File
@@ -2,6 +2,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
uint64 timestamp
uint64 heartbeat_time # Time of last received heartbeat from remote system
+33
View File
@@ -67,6 +67,7 @@ private:
#define PX4_MAX_DEV 500
static px4_dev_t *devmap[PX4_MAX_DEV];
pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
/*
* The standard NuttX operation dispatch table can't call C++ member functions
@@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data)
// Make sure the device does not already exist
// FIXME - convert this to a map for efficiency
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
pthread_mutex_unlock(&devmutex);
return -EEXIST;
}
}
@@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data)
}
}
pthread_mutex_unlock(&devmutex);
if (ret != PX4_OK) {
PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
}
@@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name)
return -EINVAL;
}
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
delete devmap[i];
@@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name)
}
}
pthread_mutex_unlock(&devmutex);
return ret;
}
@@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
pthread_mutex_lock(&devmutex);
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
delete devmap[i];
PX4_DEBUG("Unregistered class DEV %s", name);
devmap[i] = NULL;
pthread_mutex_unlock(&devmutex);
return PX4_OK;
}
}
pthread_mutex_unlock(&devmutex);
return -EINVAL;
}
@@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path)
PX4_DEBUG("VDev::getDev");
int i = 0;
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path);
//}
if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
pthread_mutex_unlock(&devmutex);
return (VDev *)(devmap[i]->cdev);
}
}
pthread_mutex_unlock(&devmutex);
return NULL;
}
@@ -514,11 +535,15 @@ void VDev::showDevices()
int i = 0;
PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
void VDev::showTopics()
@@ -526,11 +551,15 @@ void VDev::showTopics()
int i = 0;
PX4_INFO("Devices:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
void VDev::showFiles()
@@ -538,12 +567,16 @@ void VDev::showFiles()
int i = 0;
PX4_INFO("Files:");
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
pthread_mutex_unlock(&devmutex);
}
const char *VDev::topicList(unsigned int *next)
+14 -17
View File
@@ -2090,12 +2090,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (buflen > 0);
/* ensure a closing newline */
int ret;
/* send the closing newline */
msg->text[0] = '\n';
msg->text[1] = '\0';
int ret;
for (int i = 0; i < 30; i++) {
/* failed, but give it a 2nd shot */
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
@@ -2108,27 +2108,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
}
}
if (ret) {
return ret;
if (ret == 0) {
/* success, exit */
break;
}
retries--;
DEVICE_LOG("mixer sent");
} while (retries > 0);
} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
if (retries == 0) {
mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
/* load must have failed for some reason */
return -EINVAL;
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
return 0;
} else {
/* all went well, set the mixer ok flag */
return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK);
}
DEVICE_LOG("mixer rejected by IO");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
/* load must have failed for some reason */
return -EINVAL;
}
void
+1 -1
View File
@@ -33,7 +33,7 @@ add_custom_target(run_config
add_dependencies(run_config mainapp)
foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb)
foreach(debugger none gdb lldb ddd valgrind)
foreach(model none iris vtol)
if (debugger STREQUAL "none")
if (model STREQUAL "none")
-1
View File
@@ -37,7 +37,6 @@ px4_add_module(
SRCS
LaunchDetector.cpp
CatapultLaunchMethod.cpp
launchdetection_params.c
DEPENDS
platforms__common
)
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Catapult launch detection parameters, accessible via MAVLink
*
+3 -4
View File
@@ -49,7 +49,7 @@
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
#include "matrix/math.hpp"
#endif
#include <platforms/px4_defines.h>
@@ -339,9 +339,8 @@ public:
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, M, N> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
Matrix<M, N> res(MyInverse.data());
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
Matrix<M, N> res(Me.I().data());
return res;
#endif
}
+1
Submodule src/lib/matrix added at 2c7a375e3d
@@ -54,8 +54,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -117,22 +120,28 @@ private:
int _sensors_sub = -1;
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
struct {
param_t w_acc;
param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
@@ -140,11 +149,18 @@ private:
float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
vision_position_estimate_s _vision = {};
Vector<3> _vision_hdg;
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -168,6 +184,7 @@ private:
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_mag(3),
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
_lp_yaw_rate(250.0f, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
}
/**
@@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -374,6 +397,47 @@ void AttitudeEstimatorQ::task_main()
}
}
// Update vision and motion capture heading
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(_mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
math::Quaternion q(_vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transposed() * v;
}
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
math::Quaternion q(_mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transposed() * v;
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
@@ -431,12 +495,9 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +522,34 @@ void AttitudeEstimatorQ::task_main()
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* Attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* Attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
}
}
}
@@ -478,6 +567,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +580,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
}
}
@@ -545,12 +636,34 @@ bool AttitudeEstimatorQ::update(float dt)
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
if (_ext_hdg_mode > 0 && _ext_hdg_good) {
if (_ext_hdg_mode == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
if (_ext_hdg_mode == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
}
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
/**
* Complimentary filter external heading weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
/**
* Complimentary filter gyroscope bias weight
*
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
*/
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
/**
* External heading usage mode (from Motion capture/Vision)
* Set to 1 to use heading estimate from vision.
* Set to 2 to use heading from motion capture.
*
* @group Attitude Q estimator
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
/**
* Enable acceleration compensation based on GPS
* velocity.
+15 -6
View File
@@ -180,6 +180,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
static bool _usb_telemetry_active = false;
static hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter;
@@ -200,7 +201,7 @@ static struct home_position_s _home;
static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man;
struct vtol_vehicle_status_s vtol_status;
static struct vtol_vehicle_status_s vtol_status;
/**
* The daemon app only briefly exists to start
@@ -403,11 +404,11 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason > 0) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n");
}
void print_status()
@@ -1463,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
_usb_telemetry_active = true;
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
@@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[])
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
/* if the USB hardware connection went away, reboot */
if (status.usb_connected && !system_power.usb_connected) {
/*
* apparently the USB cable went away but we are still powered,
* so lets reset to a classic non-usb state.
*/
usleep(100000);
mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
usleep(400000);
px4_systemreset(false);
}
status.usb_connected = system_power.usb_connected;
/* finally judge the USB connected state based on software detection */
status.usb_connected = _usb_telemetry_active;
}
}
@@ -3053,6 +3060,8 @@ void *commander_low_prio_loop(void *arg)
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
} else {
warnx("settings saved.");
}
need_param_autosave = false;
@@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@@ -320,6 +323,12 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
/* publish the attitude */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
@@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;
@@ -1672,8 +1747,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
PX4_INFO(".");
}
PX4_INFO(" ");
return 0;
}
@@ -48,6 +48,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field
@@ -58,7 +58,6 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
@@ -242,6 +239,11 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R;
float _roll;
float _pitch;
float _yaw;
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
@@ -269,12 +271,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
void vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_debug(false)
{
/* safely initialize structs */
_att = {};
_ctrl_state = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
@@ -68,14 +68,13 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@@ -222,6 +219,9 @@ private:
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
float _roll;
float _pitch;
float _yaw;
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@@ -356,14 +356,9 @@ private:
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
* Check for changes in control state.
*/
bool vehicle_airspeed_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
void control_state_poll();
/**
* Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_nav_capabilities_pub(nullptr),
/* states */
_att(),
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
/* check if there is an airspeed update or if it timed out */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
FixedwingPositionControl::control_state_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = PX4_R(_att.R, i, j);
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* set rotation matrix and euler angles */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R_nb = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R_nb.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
}
void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
float airspeed;
if (_airspeed_valid) {
airspeed = _airspeed.true_airspeed_m_s;
airspeed = _ctrl_state.airspeed;
} else {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
@@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
land_noreturn_horizontal = true;
@@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
@@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
@@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
@@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main()
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
control_state_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
@@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
_ctrl_state.airspeed, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
@@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
Q(X_bz, X_bz) = _pn_b_noise_power.get();
// continuous time kalman filter prediction
Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
// only predict for components we have
// valid measurements for
@@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
_flowY += global_speed[1] * dt;
// measurement
Vector2f y;
Vector<float, 2> y;
y(0) = _flowX;
y(1) = _flowY;
// residual
Vector2f r = y - C * _x;
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
}
// measurement
Matrix<float, n_y_sonar, 1> y;
Vector<float, n_y_sonar> y;
y(0) = (d - _sonarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_sonar, 1> r = y - C * _x;
Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_sonar>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
void BlockLocalPositionEstimator::correctBaro()
{
Matrix<float, n_y_baro, 1> y;
Vector<float, n_y_baro> y;
y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
// baro measurement matrix
@@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_baro, 1> r = y - (C * _x);
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
}
// lower baro trust
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
} else if (_baroFault) {
_baroFault = FAULT_NONE;
@@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
R(0, 0) = cov;
}
Matrix<float, n_y_lidar, 1> y;
Vector<float, n_y_lidar> y;
y.setZero();
y(0) = (d - _lidarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_lidar, 1> r = y - C * _x;
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
Vector<float, n_y_lidar> r = y - C * _x;
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
Matrix<float, 6, 1> y;
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
@@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
R(5, 5) = var_vz;
// residual
Matrix<float, 6, 1> r = y - C * _x;
Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
Vector<float, 6> r = y - C * _x;
Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
}
// trust GPS less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
@@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
void BlockLocalPositionEstimator::correctVision()
{
Matrix<float, 3, 1> y;
Vector<float, 3> y;
y.setZero();
y(0) = _sub_vision_pos.get().x - _visionHome(0);
y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * _x;
// fault detection
@@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
} else if (_visionFault) {
_visionFault = FAULT_NONE;
@@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
void BlockLocalPositionEstimator::correctmocap()
{
Matrix<float, n_y_mocap, 1> y;
Vector<float, n_y_mocap> y;
y.setZero();
y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
// residual
Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
Matrix<float, n_y_mocap, 1> r = y - C * _x;
// fault detection
@@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
@@ -6,7 +6,7 @@
#include <lib/geo/geo.h>
#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
#include "matrix/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
perf_counter_t _err_perf;
// state space
Matrix<float, n_x, 1> _x; // state vector
Matrix<float, n_u, 1> _u; // input vector
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
};
@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
# add matrix tests
add_subdirectory(matrix)
endif()
# use custom matrix lib instead of Eigen
@@ -1 +0,0 @@
build*/
@@ -1,13 +0,0 @@
cmake_minimum_required(VERSION 2.8)
project(matrix CXX)
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
enable_testing()
include_directories(src)
add_subdirectory(test)
@@ -1,464 +0,0 @@
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename T, size_t M, size_t N>
class Matrix
{
private:
T _data[M][N];
size_t _rows;
size_t _cols;
public:
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const T *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
Matrix(T x, T y, T z) :
_data(),
_rows(M),
_cols(N)
{
// TODO, limit to only 3x1 matrices
_data[0][0] = x;
_data[1][0] = y;
_data[2][0] = z;
}
/**
* Accessors/ Assignment etc.
*/
T *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline T operator()(size_t i) const
{
return _data[i][0];
}
inline T operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline T &operator()(size_t i)
{
return _data[i][0];
}
inline T &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
{
const Matrix<T, M, N> &self = *this;
Matrix<T, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<T, M, N> operator*(T scalar) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<T, M, N> operator+(T scalar) const
{
Matrix<T, M, N> res;
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(T scalar)
{
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(T scalar)
{
Matrix<T, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<T, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<T, N, M> transpose() const
{
Matrix<T, N, M> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
Matrix<T, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<T, M, 1> diagonal() const
{
Matrix<T, M, 1> res;
// force square for now
const Matrix<T, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
T tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
T tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <T, M, M> inverse() const
{
Matrix<T, M, M> L;
L.setIdentity();
const Matrix<T, M, M> &A = (*this);
Matrix<T, M, M> U = A;
Matrix<T, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<T, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<T, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<T, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
};
typedef Matrix<float, 2, 1> Vector2f;
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
@@ -1,15 +0,0 @@
set(tests
setIdentity
inverse
matrixMult
vectorAssignment
matrixAssignment
matrixScalarMult
transpose
)
foreach(test ${tests})
add_executable(${test}
${test}.cpp)
add_test(${test} ${test})
endforeach()
@@ -1,30 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 100;
Matrix<float, n, n> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 100; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
return 0;
}
@@ -1,29 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
m.print();
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
m2.print();
assert(m == m2);
return 0;
}
@@ -1,26 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
A.print();
A_I.print();
Matrix3f R = A * A_I;
R.print();
assert(R == I);
Matrix3f R2 = A;
R2 *= A_I;
R2.print();
assert(R2 == I);
return 0;
}
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
A.print();
A_check.print();
assert(A == A_check);
return 0;
}
@@ -1,25 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f A;
A.setIdentity();
assert(A.rows() == 3);
assert(A.cols() == 3);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
} else {
assert(A(i, j) == 0);
}
}
}
return 0;
}
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
assert(A_T == A_T_check);
return 0;
}
@@ -1,28 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
return 0;
}
-1
View File
@@ -56,7 +56,6 @@ px4_add_module(
mavlink_rate_limiter.cpp
mavlink_receiver.cpp
mavlink_ftp.cpp
mavlink_params.c
DEPENDS
platforms__common
)
+8 -1
View File
@@ -698,6 +698,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
} else {
_is_usb_uart = true;
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
}
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
printf("3DR RADIO\n");
break;
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
printf("USB CDC\n");
break;
default:
printf("UNKNOWN RADIO\n");
printf("GENERIC LINK OR RADIO\n");
break;
}
+2
View File
@@ -288,6 +288,8 @@ public:
float get_rate_mult();
float get_baudrate() { return _baudrate; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
-2
View File
@@ -31,8 +31,6 @@
*
****************************************************************************/
#include <systemlib/param/param.h>
/**
* MAVLink system ID
* @group MAVLink
+11 -4
View File
@@ -1757,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
uint8_t buf[1600];
#else
/* the serial port buffers internally as well, we just need to fit a small chunk */
uint8_t buf[32];
uint8_t buf[64];
#endif
mavlink_message_t msg;
@@ -1797,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
while (!_mavlink->_task_should_exit) {
if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) {
/*
* to avoid reading very small chunks wait for data before reading
* this is designed to target one message, so >20 bytes at a time
*/
const unsigned character_count = 20;
/* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
usleep(sleeptime);
}
}
#ifdef __PX4_POSIX
@@ -74,7 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
@@ -126,7 +126,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -145,7 +145,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -297,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@@ -319,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -599,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@@ -693,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@@ -733,7 +733,7 @@ MulticopterAttitudeControl::task_main()
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -747,7 +747,7 @@ MulticopterAttitudeControl::task_main()
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -783,8 +783,8 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* copy attitude and control state topics */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
@@ -862,7 +862,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
@@ -38,7 +38,6 @@ px4_add_module(
mc_att_control_start_nuttx.cpp
mc_att_control.cpp
mc_att_control_base.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
@@ -36,14 +36,10 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
@@ -68,7 +68,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -119,14 +119,14 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
const float alt_ctl_dz = 0.1f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -230,6 +230,9 @@ private:
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
/**
* Update our local parameter cache.
*/
@@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_mavlink_fd(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
@@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
@@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_ff.zero();
_R.identity();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
@@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
orb_check(_att_sp_sub, &updated);
@@ -855,18 +861,45 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
bool current_setpoint_valid = false;
bool previous_setpoint_valid = false;
math::Vector<3> prev_sp;
math::Vector<3> curr_sp;
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true;
}
}
if (_pos_sp_triplet.previous.valid) {
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true;
}
}
if (current_setpoint_valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
@@ -876,13 +909,8 @@ void MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
@@ -987,7 +1015,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1042,6 +1070,14 @@ MulticopterPositionControl::task_main()
}
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
@@ -1117,7 +1153,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@@ -1302,11 +1338,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1493,7 +1529,7 @@ MulticopterPositionControl::task_main()
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while arming */
@@ -1503,7 +1539,7 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it
@@ -116,11 +116,11 @@ static inline int max(int val1, int val2)
*/
static void usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n");
return;
}
+1 -16
View File
@@ -88,9 +88,6 @@ static int mixer_callback(uintptr_t handle,
static MixerGroup mixer_group(mixer_callback, 0);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
static void mixer_set_failsafe();
void
mixer_tick(void)
{
@@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
}
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
@@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length)
}
mixer_text_length = resid;
/* update failsafe values */
mixer_set_failsafe();
}
break;
@@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length)
return 0;
}
static void
void
mixer_set_failsafe()
{
/*
+2
View File
@@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit;
*/
extern void mixer_tick(void);
extern int mixer_handle_text(const void *buffer, size_t length);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
extern void mixer_set_failsafe(void);
/**
* Safety switch/LED.
+12 -1
View File
@@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
/* update failsafe values, now that the mixer is set to ok */
mixer_set_failsafe();
}
break;
+19 -1
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
@@ -1089,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vtol_vehicle_status_s vtol_status;
struct time_offset_s time_offset;
struct mc_att_ctrl_status_s mc_att_ctrl_status;
struct control_state_s ctrl_state;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1137,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ENCD_s log_ENCD;
struct log_TSYN_s log_TSYN;
struct log_MACS_s log_MACS;
struct log_CTS_s log_CTS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1179,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int encoders_sub;
int tsync_sub;
int mc_att_ctrl_status_sub;
int ctrl_state_sub;
} subs;
subs.cmd_sub = -1;
@@ -1212,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = -1;
subs.tsync_sub = -1;
subs.mc_att_ctrl_status_sub = -1;
subs.ctrl_state_sub = -1;
subs.encoders_sub = -1;
/* add new topics HERE */
@@ -1856,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(MACS);
}
/* --- CONTROL STATE --- */
if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
log_msg.msg_type = LOG_CTS_MSG;
log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
@@ -1898,7 +1916,7 @@ void sdlog2_status()
}
/**
* @return 0 if file exists
* @return true if file exists
*/
bool file_exist(const char *filename)
{
+13
View File
@@ -477,6 +477,18 @@ struct log_MACS_s {
float yaw_rate_integ;
};
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 47
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* WARNING: ID 46 is already in use for ATTC1 */
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
+3 -3
View File
@@ -850,7 +850,7 @@ Sensors::parameters_update()
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
_parameters.battery_voltage_scaling = 0.0063f;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
#elif CONFIG_ARCH_BOARD_PX4FMU_V1
_parameters.battery_voltage_scaling = 0.00459340659f;
#else
/* ensure a missing default trips a low voltage lockdown */
@@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll()
void
Sensors::parameter_update_poll(bool forced)
{
bool param_updated;
bool param_updated = false;
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
@@ -2146,7 +2146,7 @@ Sensors::task_main()
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
px4_pollfd_struct_t fds[1] = {};
/* use the gyro to pace output */
fds[0].fd = _gyro_sub[0];
@@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
char serial_buf[1024];
struct pollfd fds[2];
memset(fds, 0, sizeof(fds));
unsigned fd_count = 1;
fds[0].fd = _fd;
fds[0].events = POLLIN;
if (serial_fd >= 0) {
fds[1].fd = serial_fd;
fds[1].events = POLLIN;
+2 -2
View File
@@ -481,7 +481,7 @@ public:
* compared to thrust.
* @param yaw_wcale Scaling factor applied to yaw inputs compared
* to thrust.
* @param deadband Minumum rotor control output value; usually
* @param idle_speed Minumum rotor control output value; usually
* tuned to ensure that rotors never stall at the
* low end of their control range.
*/
@@ -491,7 +491,7 @@ public:
float roll_scale,
float pitch_scale,
float yaw_scale,
float deadband);
float idle_speed);
~MultirotorMixer();
/**
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. 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.
#
############################################################################
SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
MULTI_TABLES := $(SELF_DIR)multi_tables.py
# Add explicit dependency, as implicit one doesn't work often.
$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h
+2 -2
View File
@@ -40,7 +40,7 @@
/**
* Auto-start script index.
*
* Defines the auto-start script used to bootstrap the system.
* CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system.
*
* @group System
*/
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
/**
* Companion computer interface
*
* Configures the baud rate of the companion computer interface.
* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface.
* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
* 157600: enables OSD mode at 57600 baud, 8N1.
+3
View File
@@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s);
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
#include "topics/control_state.h"
ORB_DEFINE(control_state, struct control_state_s);
#include "topics/sensor_combined.h"
ORB_DEFINE(sensor_combined, struct sensor_combined_s);
@@ -38,12 +38,9 @@ px4_add_module(
SRCS
vtol_att_control_main.cpp
vtol_att_control_params.c
tiltrotor_params.c
tiltrotor.cpp
vtol_type.cpp
tailsitter.cpp
standard_params.c
standard.cpp
DEPENDS
platforms__common
+55 -39
View File
@@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
_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()
{
@@ -98,12 +98,12 @@ Standard::parameters_update()
void Standard::update_vtol_state()
{
parameters_update();
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.
* 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 (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
@@ -130,7 +130,7 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f)) {
(_params_standard.back_trans_dur * 1000000.0f)) {
_vtol_schedule.flight_mode = MC_MODE;
}
}
@@ -168,17 +168,19 @@ void Standard::update_vtol_state()
}
// 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;
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;
}
}
@@ -188,18 +190,21 @@ void Standard::update_transition_state()
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);
_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) {
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -210,17 +215,19 @@ void Standard::update_transition_state()
} 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) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// 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();
@@ -238,15 +245,15 @@ void Standard::update_mc_state()
// do nothing
}
void Standard::update_fw_state()
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
// 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()
{
@@ -259,22 +266,31 @@ void Standard::update_external_state()
void Standard::fill_actuator_outputs()
{
/* multirotor controls */
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
/* fixed wing controls */
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_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[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
}
}
@@ -288,11 +304,11 @@ Standard::set_max_mc(unsigned pwm_value)
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -301,9 +317,9 @@ Standard::set_max_mc(unsigned pwm_value)
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
+11 -11
View File
@@ -31,15 +31,15 @@
*
****************************************************************************/
/**
* @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>
*
*/
/**
* @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
@@ -52,7 +52,7 @@ class Standard : public VtolType
public:
Standard(VtolAttitudeControl * _att_controller);
Standard(VtolAttitudeControl *_att_controller);
~Standard();
void update_vtol_state();
@@ -89,7 +89,7 @@ private:
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;
} _vtol_schedule;
bool _flag_enable_mc_motors;
float _pusher_throttle;
@@ -36,11 +36,9 @@
* Parameters for the standard VTOL controller.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
+75 -56
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
}
@@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state()
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
} else {
_vtol_mode = FIXED_WING;
}
@@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
@@ -91,18 +92,18 @@ void Tailsitter::update_external_state()
}
void Tailsitter::calc_tot_airspeed()
{
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P,1.0f,_params->power_max);
P = math::constrain(P, 1.0f, _params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
float v_ind = (airspeed / eta - airspeed) * 2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
@@ -115,16 +116,19 @@ Tailsitter::scale_mc_output()
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
@@ -135,8 +139,10 @@ Tailsitter::scale_mc_output()
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
-1.0f, 1.0f);
}
/**
@@ -144,37 +150,50 @@ Tailsitter::scale_mc_output()
*/
void Tailsitter::fill_actuator_outputs()
{
switch(_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
switch (_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
}
}
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
@@ -48,7 +48,7 @@ class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl * _att_controller);
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
void update_vtol_state();
+143 -102
View File
@@ -44,12 +44,10 @@
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_roll_weight_mc(1.0f),
_yaw_weight_mc(1.0f),
_min_front_trans_dur(0.5f)
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_min_front_trans_dur(0.5f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -88,11 +86,11 @@ Tiltrotor::parameters_update()
/* vtol duration of a front transition */
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
_params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
/* vtol duration of a back transition */
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
_params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
@@ -125,22 +123,26 @@ Tiltrotor::parameters_update()
/* avoid parameters which will lead to zero division in the transition code */
_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
}
return OK;
}
int Tiltrotor::get_motor_off_channels(int channels) {
int Tiltrotor::get_motor_off_channels(int channels)
{
int channel_bitmap = 0;
int channel;
for (int i = 0; i < _params->vtol_motor_count; ++i) {
channel = channels % 10;
if (channel == 0) {
break;
}
channel_bitmap |= 1 << channel;
channels = channels / 10;
}
@@ -150,82 +152,98 @@ int Tiltrotor::get_motor_off_channels(int channels) {
void Tiltrotor::update_vtol_state()
{
parameters_update();
parameters_update();
/* simple logic using a two way switch to perform transitions.
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
*/
if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
break;
}
} else {
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
break;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
break;
}
}
// map tiltrotor 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_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
}
}
@@ -250,7 +268,7 @@ void Tiltrotor::update_mc_state()
_mc_yaw_weight = 1.0f;
}
void Tiltrotor::update_fw_state()
void Tiltrotor::update_fw_state()
{
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
@@ -278,15 +296,18 @@ void Tiltrotor::update_transition_state()
if (_rear_motors != ENABLED) {
set_rear_motor_state(ENABLED);
}
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
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);
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 >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -294,6 +315,7 @@ void Tiltrotor::update_transition_state()
// disable mc yaw control once the plane has picked up speed
_mc_yaw_weight = 1.0f;
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
@@ -301,8 +323,10 @@ void Tiltrotor::update_transition_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_roll_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_rear_motors != IDLE) {
set_rear_motor_state(IDLE);
@@ -312,10 +336,12 @@ void Tiltrotor::update_transition_state()
set_idle_mc();
flag_idle_mc = true;
}
// tilt rotors back
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw -
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
}
// set zero throttle for backtransition otherwise unwanted moments will be created
@@ -339,19 +365,28 @@ void Tiltrotor::update_external_state()
*/
void Tiltrotor::fill_actuator_outputs()
{
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
}
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[4] = _tilt_control;
}
@@ -360,52 +395,58 @@ void Tiltrotor::fill_actuator_outputs()
* Set state of rear motors.
*/
void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
void Tiltrotor::set_rear_motor_state(rear_motor_state state)
{
int pwm_value = PWM_DEFAULT_MAX;
// map desired rear rotor state to max allowed pwm signal
switch (state) {
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
}
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_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++) {
if (is_motor_off_channel(i)) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = PWM_DEFAULT_MAX;
}
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
bool Tiltrotor::is_motor_off_channel(const int channel) {
bool Tiltrotor::is_motor_off_channel(const int channel)
{
return (_params_tiltrotor.fw_motors_off >> channel) & 1;
}
+8 -10
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TILTROTOR_H
#define TILTROTOR_H
@@ -49,7 +49,7 @@ class Tiltrotor : public VtolType
public:
Tiltrotor(VtolAttitudeControl * _att_controller);
Tiltrotor(VtolAttitudeControl *_att_controller);
~Tiltrotor();
/**
@@ -127,11 +127,9 @@ private:
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
}_vtol_schedule;
} _vtol_schedule;
float _tilt_control; /**< actuator value for the tilt servo */
float _roll_weight_mc; /**< multicopter desired roll moment weight */
float _yaw_weight_mc; /**< multicopter desired yaw moment weight */
const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
@@ -35,7 +35,7 @@
* @file tiltrotor_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
@@ -93,10 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
memset(&_armed, 0, sizeof(_armed));
memset(&_local_pos,0,sizeof(_local_pos));
memset(&_airspeed,0,sizeof(_airspeed));
memset(&_batt_status,0,sizeof(_batt_status));
memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_airspeed, 0, sizeof(_airspeed));
memset(&_batt_status, 0, sizeof(_batt_status));
memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
_params.idle_pwm_mc = PWM_LOWEST_MIN;
_params.vtol_motor_count = 0;
@@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() :
if (_params.vtol_type == 0) {
_tailsitter = new Tailsitter(this);
_vtol_type = _tailsitter;
} 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;
}
@@ -150,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
* Check for airspeed updates.
*/
void
VtolAttitudeControl::vehicle_airspeed_poll() {
VtolAttitudeControl::vehicle_airspeed_poll()
{
bool updated;
orb_check(_airspeed_sub, &updated);
@@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() {
* Check for battery updates.
*/
void
VtolAttitudeControl::vehicle_battery_poll() {
VtolAttitudeControl::vehicle_battery_poll()
{
bool updated;
orb_check(_battery_status_sub, &updated);
@@ -442,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
void VtolAttitudeControl::task_main()
{
warnx("started");
PX4_WARN("started");
fflush(stdout);
/* do subscriptions */
@@ -471,7 +476,7 @@ void VtolAttitudeControl::task_main()
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
@@ -491,7 +496,7 @@ void VtolAttitudeControl::task_main()
}
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
@@ -633,7 +638,7 @@ void VtolAttitudeControl::task_main()
warnx("exit");
_control_task = -1;
_exit(0);
return;
}
int
@@ -643,14 +648,14 @@ VtolAttitudeControl::start()
/* start the task */
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(px4_main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
PX4_WARN("task start failed");
return -errno;
}
@@ -661,49 +666,54 @@ VtolAttitudeControl::start()
int vtol_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: vtol_att_control {start|stop|status}");
PX4_WARN("usage: vtol_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (VTOL_att_control::g_control != nullptr) {
errx(1, "already running");
PX4_WARN("already running");
return 0;
}
VTOL_att_control::g_control = new VtolAttitudeControl;
if (VTOL_att_control::g_control == nullptr) {
errx(1, "alloc failed");
PX4_WARN("alloc failed");
return 1;
}
if (OK != VTOL_att_control::g_control->start()) {
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
err(1, "start failed");
PX4_WARN("start failed");
return 1;
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (VTOL_att_control::g_control == nullptr) {
errx(1, "not running");
PX4_WARN("not running");
return 0;
}
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (VTOL_att_control::g_control) {
errx(0, "running");
PX4_WARN("running");
} else {
errx(1, "not running");
PX4_WARN("not running");
}
return 0;
}
warnx("unrecognized command");
PX4_WARN("unrecognized command");
return 1;
}
@@ -46,6 +46,10 @@
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -80,7 +84,6 @@
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
#include "tiltrotor.h"
@@ -101,24 +104,24 @@ public:
int start(); /* start the task and return OK on success */
bool is_fixed_wing_requested();
struct vehicle_attitude_s* get_att () {return &_v_att;}
struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;}
struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;}
struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;}
struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;}
struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;}
struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;}
struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;}
struct actuator_armed_s* get_armed () {return &_armed;}
struct vehicle_local_position_s* get_local_pos () {return &_local_pos;}
struct airspeed_s* get_airspeed () {return &_airspeed;}
struct battery_status_s* get_batt_status () {return &_batt_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_armed_s *get_armed() {return &_armed;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct Params* get_params () {return &_params;}
struct Params *get_params() {return &_params;}
private:
@@ -184,17 +187,12 @@ private:
param_t elevons_mc_lock;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
unsigned _motor_count; // number of motors
float _airspeed_tot;
int _transition_command;
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
Standard * _standard; // standard vtol type
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***********************************************************************
@@ -35,11 +35,9 @@
* @file vtol_att_control_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstr@ethz.ch>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* VTOL number of engines
*
+21 -21
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include <px4_defines.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
@@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING)
VtolType::~VtolType()
{
}
/**
@@ -81,11 +81,11 @@ void VtolType::set_idle_mc()
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -95,11 +95,11 @@ void VtolType::set_idle_mc()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
flag_idle_mc = true;
}
@@ -111,9 +111,9 @@ void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
@@ -125,9 +125,9 @@ void VtolType::set_idle_fw()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
}
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H
@@ -85,7 +85,7 @@ public:
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
mode get_mode() {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;
+1
View File
@@ -35,6 +35,7 @@ set(depends
prebuild_targets
git_mavlink
git_uavcan
git_matrix
)
px4_add_module(
@@ -111,6 +111,7 @@ private:
ADCSIM::ADCSIM(uint32_t channels) :
VDev("adcsim", ADCSIM0_DEVICE_PATH),
_call(),
_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
_channel_count(0),
_samples(nullptr)
+11 -23
View File
@@ -81,35 +81,23 @@ static void hrt_unlock(void)
px4_sem_post(&_hrt_lock);
}
#ifdef __PX4_DARWIN
#include <mach/mach_time.h>
#define MAC_NANO (+1.0E-9)
#define MAC_GIGA UINT64_C(1000000000)
#define CLOCK_MONOTONIC 1
#define HRT_LOCK_NAME "/hrt_lock"
static double px4_timebase = 0.0;
#if defined(__APPLE__) && defined(__MACH__)
#include <time.h>
#include <sys/time.h>
#define CLOCK_REALTIME 0
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
{
if (clk_id != CLOCK_MONOTONIC) {
return 1;
struct timeval now;
int rv = gettimeofday(&now, NULL);
if (rv) {
return rv;
}
if (!px4_timestart) {
mach_timebase_info_data_t tb = {};
mach_timebase_info(&tb);
px4_timebase = tb.numer;
px4_timebase /= tb.denom;
// px4_timestart = mach_absolute_time();
}
tp->tv_sec = now.tv_sec;
tp->tv_nsec = now.tv_usec * 1000;
memset(tp, 0, sizeof(*tp));
double diff = mach_absolute_time() * px4_timebase;
tp->tv_sec = diff * MAC_NANO;
tp->tv_nsec = diff - (tp->tv_sec * MAC_GIGA);
return 0;
}
@@ -81,9 +81,7 @@ typedef struct {
static void *entry_adapter(void *ptr)
{
pthdata_t *data;
data = (pthdata_t *) ptr;
pthdata_t *data = (pthdata_t *) ptr;
data->entry(data->argc, data->argv);
free(ptr);
PX4_DEBUG("Before px4_task_exit");
+4
View File
@@ -58,6 +58,7 @@
#include <px4_vehicle_rates_setpoint.h>
#include <px4_mc_virtual_rates_setpoint.h>
#include <px4_vehicle_attitude.h>
#include <px4_control_state.h>
#include <px4_vehicle_control_mode.h>
#include <px4_actuator_armed.h>
#include <px4_parameter_update.h>
@@ -87,6 +88,7 @@
#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
#include <platforms/nuttx/px4_messages/px4_control_state.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
@@ -120,6 +122,7 @@
#include <platforms/posix/px4_messages/px4_actuator_controls_3.h>
#include <platforms/posix/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/posix/px4_messages/px4_vehicle_attitude.h>
#include <platforms/posix/px4_messages/px4_control_state.h>
#include <platforms/posix/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/posix/px4_messages/px4_actuator_armed.h>
#include <platforms/posix/px4_messages/px4_parameter_update.h>
@@ -150,6 +153,7 @@
#include <platforms/qurt/px4_messages/px4_actuator_controls_3.h>
#include <platforms/qurt/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/qurt/px4_messages/px4_vehicle_attitude.h>
#include <platforms/qurt/px4_messages/px4_control_state.h>
#include <platforms/qurt/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/qurt/px4_messages/px4_actuator_armed.h>
#include <platforms/qurt/px4_messages/px4_parameter_update.h>
+4 -4
View File
@@ -93,12 +93,12 @@ mixer_main(int argc, char *argv[])
static void
usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage:\n");
PX4_INFO(" mixer load <device> <filename>\n");
PX4_INFO("usage:");
PX4_INFO(" mixer load <device> <filename>");
}
static int
View File
View File