diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 new file mode 100644 index 0000000000..b27dc9b3e3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -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 +# + +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 + diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 60761ed511..4aa58447f3 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -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) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9a6581437e..719bacb41f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a9ffbe0c42..7af8fa3954 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -56,6 +56,8 @@ #include #include #include +#include +#include #include #include @@ -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 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d3959db4cf..41b62f04e1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -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. diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a465cc64f5..17bcb0f967 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b15ec09610..0a0962367d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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() { /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a5e7a5500c..51ed9c6e40 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 46852003c8..18f5db5645 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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;