diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-16 16:10:57 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-16 16:10:57 +0400 |
commit | d736ed181f51944c3a12ea032da34dfa59961eea (patch) | |
tree | 2ff24f167ca13d6b1cba52500030ded97e0d5382 | |
parent | 550c611a6e2bf0affc7063adb1c6bae66dd3d4ca (diff) | |
parent | 8c24299a40804182e60f38dfd6bf70bfa7d034ba (diff) | |
download | px4-firmware-d736ed181f51944c3a12ea032da34dfa59961eea.tar.gz px4-firmware-d736ed181f51944c3a12ea032da34dfa59961eea.tar.bz2 px4-firmware-d736ed181f51944c3a12ea032da34dfa59961eea.zip |
Merge branch 'master' into vector_control
20 files changed, 394 insertions, 128 deletions
diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 6be917878..f6b071cf1 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -67,8 +67,7 @@ pwm rate -c 1234 -r 400 # Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1050 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 4686382ca..8e0914d63 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -103,13 +103,6 @@ else fi # -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk new file mode 100644 index 000000000..e1a42530b --- /dev/null +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -0,0 +1,160 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4io +MODULES += drivers/px4fmu +MODULES += drivers/boards/px4fmu-v1 +MODULES += drivers/ardrone_interface +MODULES += drivers/l3gd20 +MODULES += drivers/bma180 +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/rgbled +MODULES += drivers/mkblctrl +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/eeprom +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/i2c +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard +MODULES += modules/gpio_led + +# +# Estimation modules (EKF / other filters) +# +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/att_pos_estimator_ekf +#MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control +#MODULES += modules/multirotor_att_control +#MODULES += modules/multirotor_pos_control +#MODULES += examples/flow_position_control +#MODULES += examples/flow_speed_control +MODULES += modules/fixedwing_backside + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 048784813..62c0d1f17 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - uint16_t _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9d8ad084e..de371bf32 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -180,7 +180,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = diff_pres_pa; + report.differential_pressure_pa = (float)diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b3003fc1b..3cd6d6720 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -36,6 +36,7 @@ * @author Lorenz Meier * @author Sarthak Kaingade * @author Simon Wilks + * @author Thomas Gubler * * Driver for the MEAS Spec series connected via I2C. * @@ -76,6 +77,7 @@ #include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> +#include <mathlib/mathlib.h> #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> @@ -184,7 +186,7 @@ MEASAirspeed::collect() //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; + dp_raw = 0x3FFF & dp_raw; //mask the used bits dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; @@ -193,7 +195,11 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value - uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); +// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + const float P_min = -1.0f; + const float P_max = 1.0f; + float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 18fb6dcbc..ecca04dd7 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -41,6 +41,7 @@ #include "KalmanNav.hpp" #include <systemlib/err.h> +#include <geo/geo.h> // constants // Titterton pg. 52 @@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // attitude representations C_nb(), q(), - _accel_sub(-1), - _gyro_sub(-1), - _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), @@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), // parameters for ground station _vGyro(this, "V_GYRO"), _vAccel(this, "V_ACCEL"), @@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // gyro, accel and mag subscriptions - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - struct accel_report accel; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); - - struct mag_report mag; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); - // initialize rotation quaternion with a single raw sensor measurement - q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); // initialize dcm C_nb = Dcm(q); @@ -252,11 +248,21 @@ void KalmanNav::update() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + // XXX map_projection has internal global + // states that multiple things could change, + // should make map_projection take reference + // lat/lon and not have init + map_projection_init(lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS\n"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), - lat, lon, alt); + lat, lon, double(alt)); } // prediction step @@ -311,7 +317,7 @@ void KalmanNav::updatePublications() { using namespace math; - // position publication + // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; @@ -324,6 +330,31 @@ void KalmanNav::updatePublications() _pos.vz = vD; _pos.yaw = psi; + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = getLatDegE7(); + _localPos.ref_lon = getLonDegE7(); + _localPos.ref_alt = 0; + _localPos.landed = landed; + // attitude publication _att.timestamp = _pubTimeStamp; _att.roll = phi; @@ -347,8 +378,10 @@ void KalmanNav::updatePublications() // selectively update publications, // do NOT call superblock do-all method - if (_positionInitialized) + if (_positionInitialized) { _pos.update(); + _localPos.update(); + } if (_attitudeInitialized) _att.update(); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 799fc2fb9..a69bde1a6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/parameter_update.h> @@ -142,12 +143,8 @@ protected: control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ // publications control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ - - int _accel_sub; /**< Accelerometer subscription */ - int _gyro_sub; /**< Gyroscope subscription */ - int _mag_sub; /**< Magnetometer subscription */ - // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -164,8 +161,10 @@ protected: float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ double lat, lon; /**< lat, lon radians */ - float alt; /**< altitude, meters */ // parameters + float alt; /**< altitude, meters */ + double lat0, lon0; /**< reference latitude and longitude */ + float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66..1809f9688 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -37,12 +37,15 @@ */ #include "airspeed_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include <stdio.h> +#include <fcntl.h> #include <poll.h> #include <math.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_airspeed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/differential_pressure.h> #include <mavlink/mavlink_log.h> @@ -55,10 +58,13 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "dpress"; + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); const int calibration_count = 2500; @@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd) int calibration_counter = 0; float diff_pres_offset = 0.0f; + /* Reset sensor parameters */ + struct airspeed_scale airscale = { + 0.0f, + 1.0f, + }; + + bool paramreset_successful = false; + int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { + if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + paramreset_successful = true; + } + close(fd); + } + + if (!paramreset_successful) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } @@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } @@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } - mavlink_log_info(mavlink_fd, "airspeed calibration done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(); close(diff_pres_sub); return OK; } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d65045d68..6dc19df41 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { + // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || - _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between + // calculate velocity, XXX should be airspeed, + // but using ground speed for now for the purpose + // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vx * _pos.vx + @@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update() // a first binary release can be targeted. // This is not a hack, but a design choice. - /* do not limit in HIL */ + // do not limit in HIL if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + } else if (_status.main_state == MAIN_STATE_MANUAL) { _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + + } else if (_status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 3876e4630..567efeb35 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -255,13 +255,13 @@ private: BlockWaypointGuidance _guide; // block params - BlockParam<float> _trimAil; - BlockParam<float> _trimElv; - BlockParam<float> _trimRdr; - BlockParam<float> _trimThr; - BlockParam<float> _trimV; - BlockParam<float> _vCmd; - BlockParam<float> _crMax; + BlockParamFloat _trimAil; + BlockParamFloat _trimElv; + BlockParamFloat _trimRdr; + BlockParamFloat _trimThr; + BlockParamFloat _trimV; + BlockParamFloat _vCmd; + BlockParamFloat _crMax; struct pollfd _attPoll; vehicle_global_position_set_triplet_s _lastPosCmd; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c51a6de08..bfccd5fb0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -72,6 +72,7 @@ #include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> #include <commander/px4_custom_mode.h> +#include <geo/geo.h> __BEGIN_DECLS @@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; struct vehicle_global_position_s hil_global_pos; +struct vehicle_local_position_s hil_local_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_local_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; @@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; +// variables for HIL reference position +static int32_t lat0 = 0; +static int32_t lon0 = 0; +static double alt0 = 0; + static void handle_message(mavlink_message_t *msg) { @@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - hil_global_pos.valid = true; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; - - /* set timestamp and notify processes (broadcast) */ - hil_global_pos.timestamp = hrt_absolute_time(); + uint64_t timestamp = hrt_absolute_time(); + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + // global position packet + hil_global_pos.timestamp = timestamp; + hil_global_pos.valid = true; + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } + // publish local position + if (pub_hil_local_pos > 0) { + float x; + float y; + bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + double lat = hil_state.lat*1e-7; + double lon = hil_state.lon*1e-7; + map_projection_project(lat, lon, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = alt0 - hil_state.alt/1000.0f; + hil_local_pos.vx = hil_state.vx/100.0f; + hil_local_pos.vy = hil_state.vy/100.0f; + hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = alt0; + hil_local_pos.landed = landed; + orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { + pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + lat0 = hil_state.lat; + lon0 = hil_state.lon; + alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + } + /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); math::Dcm C_nb(q); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index d37b18a19..fa23f996f 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -64,6 +64,7 @@ #include <systemlib/systemlib.h> #include <mavlink/mavlink_log.h> +#include "geo/geo.h" #include "waypoints.h" #include "orb_topics.h" #include "missionlib.h" diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index f6860930c..abc91d34f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -75,6 +75,7 @@ struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_effective_0; struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; +struct airspeed_s airspeed; struct mavlink_subscriptions mavlink_subs; @@ -92,6 +93,8 @@ static unsigned int gps_counter; */ static uint64_t last_sensor_timestamp; +static hrt_abstime last_sent_vfr = 0; + static void *uorb_receive_thread(void *arg); struct listener { @@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l) /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - if (gcs_link) + if (gcs_link) { /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, @@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l) att.pitchspeed, att.yawspeed); + /* limit VFR message rate to 10Hz */ + hrt_abstime t = hrt_absolute_time(); + if (t >= last_sent_vfr + 100000) { + last_sent_vfr = t; + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + } + } + attitude_counter++; } @@ -681,17 +695,7 @@ l_home(const struct listener *l) void l_airspeed(const struct listener *l) { - struct airspeed_s airspeed; - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); - - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.relative_alt; - float climb = -global_pos.vz; - - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } void @@ -849,7 +853,7 @@ uorb_receive_start(void) mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ - /* --- AIRSPEED / VFR / HUD --- */ + /* --- AIRSPEED --- */ mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index adb63186c..86ac0e4ff 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,31 +59,23 @@ #include <systemlib/err.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; float yawrate_d; float yawrate_i; - //float yawrate_awu; - //float yawrate_lim; float attrate_p; float attrate_d; float attrate_i; - //float attrate_awu; - //float attrate_lim; float rate_lim; }; @@ -93,14 +85,10 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_d; - //param_t yawrate_awu; - //param_t yawrate_lim; param_t attrate_p; param_t attrate_i; param_t attrate_d; - //param_t attrate_awu; - //param_t attrate_lim; }; /** @@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h) h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_d = param_find("MC_YAWRATE_D"); - //h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - //h->yawrate_lim = param_find("MC_YAWRATE_LIM"); h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_d = param_find("MC_ATTRATE_D"); - //h->attrate_awu = param_find("MC_ATTRATE_AWU"); - //h->attrate_lim = param_find("MC_ATTRATE_LIM"); return OK; } @@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_d, &(p->yawrate_d)); - //param_get(h->yawrate_awu, &(p->yawrate_awu)); - //param_get(h->yawrate_lim, &(p->yawrate_lim)); param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_d, &(p->attrate_d)); - //param_get(h->attrate_awu, &(p->attrate_awu)); - //param_get(h->attrate_lim, &(p->attrate_lim)); return OK; } @@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static PID_t pitch_rate_controller; static PID_t roll_rate_controller; + static PID_t yaw_rate_controller; static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } /* load new parameters with lower rate */ @@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); } /* reset integrals if needed */ if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); - // TODO pid_reset_integral(&yaw_rate_controller); + pid_reset_integral(&yaw_rate_controller); } - /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); - - /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); - - /* control yaw rate */ //XXX use library here - float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); - - /* increase resilience to faulty control inputs */ - if (!isfinite(yaw_rate_control)) { - yaw_rate_control = 0.0f; - warnx("rej. NaN ctrl yaw"); - } + /* run pitch, roll and yaw controllers */ + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; + actuators->control[2] = yaw_control; actuators->control[3] = rate_sp->thrust; motor_skip_counter++; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 36dd370fb..3d23d0c09 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } - att_sp.yaw_body = global_pos_sp.yaw; + /* update yaw setpoint only if value is valid */ + if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { + att_sp.yaw_body = global_pos_sp.yaw; + } mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372..6d38b98ec 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) warn("WARNING: failed to set scale / offsets for airspeed sensor"); + close(fd); } #if 0 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60f..1a479c205 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -59,7 +59,7 @@ float calc_indicated_airspeed(float differential_pressure) { - if (differential_pressure > 0) { + if (differential_pressure > 0.0f) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ce..5d94d4288 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,8 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - uint16_t differential_pressure_pa; /**< Differential pressure reading */ - uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 09a934400..898c04cd5 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); + /* get current servo values */ + struct pwm_output_values last_spos; + + for (unsigned i = 0; i < servo_count; i++) { + + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_GET(%d)", i); + } + /* perform PWM output */ /* Open console directly to grab CTRL-C signal */ @@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[]) read(0, &c, 1); if (c == 0x03 || c == 0x63 || c == 'q') { + /* reset output to the last value */ + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + } warnx("User abort\n"); exit(0); } |