forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into matrix
This commit is contained in:
jgoppert
commit
4345d8db6e
|
|
@ -0,0 +1,34 @@
|
|||
#!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.08
|
||||
param set MC_ROLLRATE_I 0.05
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
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 1075
|
||||
param set PWM_THR_MIN 0.06
|
||||
param set PWM_MANTHR_MIN 0.06
|
||||
fi
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -56,6 +56,8 @@
|
|||
#include <uORB/topics/vehicle_attitude.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 +119,27 @@ 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;
|
||||
|
||||
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 +147,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 +182,7 @@ private:
|
|||
bool _data_good = false;
|
||||
bool _failsafe = false;
|
||||
bool _vibration_warning = false;
|
||||
bool _ext_hdg_good = false;
|
||||
|
||||
int _mavlink_fd = -1;
|
||||
|
||||
|
|
@ -196,12 +211,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
|
|||
|
||||
_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 +286,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 +395,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);
|
||||
|
||||
|
|
@ -478,6 +540,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 +553,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 +609,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.
|
||||
|
|
|
|||
|
|
@ -855,18 +855,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 +903,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) {
|
||||
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
{
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue