diff options
Diffstat (limited to 'src')
57 files changed, 2559 insertions, 1733 deletions
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b7981e9c4..307f7dbc7 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -149,6 +149,8 @@ enum { TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, + TONE_EKF_WARNING_TUNE, + TONE_BARO_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 6ab437716..159706278 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -92,7 +92,7 @@ #include <drivers/airspeed/airspeed.h> /* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ #define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ @@ -102,9 +102,9 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ -#define MEAS_RATE 100.0f -#define MEAS_DRIVER_FILTER_FREQ 3.0f -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 1.2f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed { diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c39494fb0..d227e15d5 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -157,6 +157,10 @@ private: perf_counter_t _pc_idle; perf_counter_t _pc_badidle; + /* do not allow top copying this class */ + PX4IO_serial(PX4IO_serial &); + PX4IO_serial& operator = (const PX4IO_serial &); + }; IOPacket PX4IO_serial::_dma_buffer; @@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _bus_semaphore(SEM_INITIALIZER(0)), + _completion_semaphore(SEM_INITIALIZER(0)), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index bf6893a7e..fb16f891f 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -65,7 +65,8 @@ PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), - _fw_fd(-1) + _fw_fd(-1), + bl_rev(0) { } @@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout) { struct pollfd fds[1]; @@ -262,19 +263,19 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) return -ETIMEDOUT; } - read(_io_fd, &c, 1); + read(_io_fd, c, 1); #ifdef UDEBUG - log("recv 0x%02x", c); + log("recv_bytes 0x%02x", c); #endif return OK; } int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) +PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { - int ret; + int ret = OK; while (count--) { - ret = recv(*p++, 5000); + ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) break; @@ -289,10 +290,10 @@ PX4IO_Uploader::drain() int ret; do { - // the small recv timeout here is to allow for fast + // the small recv_bytes timeout here is to allow for fast // drain when rebooting the io board for a forced // update of the fw without using the safety switch - ret = recv(c, 40); + ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG if (ret == OK) { @@ -331,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout) uint8_t c[2]; int ret; - ret = recv(c[0], timeout); + ret = recv_byte_with_timeout(c, timeout); if (ret != OK) return ret; - ret = recv(c[1], timeout); + ret = recv_byte_with_timeout(c + 1, timeout); if (ret != OK) return ret; @@ -372,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(param); send(PROTO_EOC); - ret = recv((uint8_t *)&val, sizeof(val)); + ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) return ret; @@ -513,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) for (ssize_t i = 0; i < count; i++) { uint8_t c; - ret = recv(c, 5000); + ret = recv_byte_with_timeout(&c, 5000); if (ret != OK) { log("%d: got %d waiting for bytes", sent + i, ret); @@ -600,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 3e2142cf2..e17523413 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -74,19 +74,19 @@ private: INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ - PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ + PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */ }; int _io_fd; int _fw_fd; - uint32_t bl_rev; /**< bootloader revision */ + uint32_t bl_rev; /**< bootloader revision */ void log(const char *fmt, ...); - int recv(uint8_t &c, unsigned timeout); - int recv(uint8_t *p, unsigned count); + int recv_byte_with_timeout(uint8_t *c, unsigned timeout); + int recv_bytes(uint8_t *p, unsigned count); void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aacc..8f523b390 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -336,6 +336,8 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP"; _default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release + _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning + _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -348,6 +350,8 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning _tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm _tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release + _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning + _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning } ToneAlarm::~ToneAlarm() diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index f2aa3db6a..93a5b511f 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ l1/ecl_l1_pos_controller.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 53f1629e3..29d3514f6 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -46,3 +46,5 @@ # SRCS = tecs/tecs.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6386e37a0..a57a0481a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; @@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM _throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; } + // Ensure _last_throttle_dem is always initialized properly + // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! + _last_throttle_dem = _throttle_dem; + // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand @@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _tecs_state.mode = ECL_TECS_MODE_NORMAL; + } + + _tecs_state.hgt_dem = _hgt_dem_adj; + _tecs_state.hgt = _integ3_state; + _tecs_state.dhgt_dem = _hgt_rate_dem; + _tecs_state.dhgt = _integ2_state; + _tecs_state.spd_dem = _TAS_dem_adj; + _tecs_state.spd = _integ5_state; + _tecs_state.dspd = _vel_dot; + _tecs_state.ithr = _integ6_state; + _tecs_state.iptch = _integ7_state; + _tecs_state.thr = _throttle_dem; + _tecs_state.ptch = _pitch_dem; + _tecs_state.dspd_dem = _TAS_rate_dem; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c79..bcc2d90e5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,27 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), @@ -100,29 +121,42 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); - - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // float hgt; - // float dhgt; - // float hgt_dem; - // float dhgt_dem; - // float spd_dem; - // float spd; - // float dspd; - // float ithr; - // float iptch; - // float thr; - // float ptch; - // float dspd_dem; - // } log_tuning; + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + struct tecs_state { + uint64_t timestamp; + float hgt; + float dhgt; + float hgt_dem; + float dhgt_dem; + float spd_dem; + float spd; + float dspd; + float ithr; + float iptch; + float thr; + float ptch; + float dspd_dem; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -188,6 +222,9 @@ public: } private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -204,6 +241,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index eac8b0afc..2d2aeecfa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -124,7 +124,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 @@ -163,7 +163,8 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; -static float eph_epv_threshold = 5.0f; +static float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -335,6 +336,7 @@ void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -391,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -758,6 +760,7 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -999,6 +1002,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; @@ -1103,7 +1107,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1128,32 +1132,32 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1183,8 +1187,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1192,7 +1196,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { @@ -1306,14 +1310,14 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1327,7 +1331,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1386,7 +1390,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1412,7 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1524,7 +1528,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -2184,7 +2188,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2247,7 +2251,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2e18c4284..08dda2fab 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) MTT_ALL_NOT_VALID, MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, - { "transition: MANUAL to AUTO_MISSION - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, - { "transition: AUTO_MISSION to MANUAL - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..f8589d24b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = { }; transition_result_t -arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); @@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current int prearm_ret = OK; /* only perform the check if we have to */ - if (new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -669,7 +670,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) goto system_eval; } - if (!status->is_rotary_wing) { + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { /* accel done, close it */ close(fd); fd = orb_subscribe(ORB_ID(airspeed)); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..69ce8bbce 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 0175acda9..f739446fa 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 37d7832b3..bffc355a8 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ccc497323..91d17e787 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -630,10 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); - // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); - // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); - // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); - // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1469,25 +1469,6 @@ FixedwingEstimator::task_main() _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - - } - - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 768e0be35..c7c7305b2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { @@ -1070,10 +1087,18 @@ void AttPosEKF::FuseVelposNED() // apply a 10-sigma threshold current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode) { current_ekf_state.hgtHealth = true; current_ekf_state.hgtFailTime = millis(); + + // if we just reset from a timeout, do not fuse + // the height data, but reset height and stored states + if (current_ekf_state.hgtTimeout) { + ResetHeight(); + ResetStoredStates(); + fuseHgtData = false; + } } else { @@ -2773,7 +2798,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) ResetHeight(); ResetStoredStates(); - ret = 3; + ret = 0; } // Reset the filter if gyro offsets are excessive @@ -3028,6 +3053,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 6d1f47b68..a6b670c4d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -68,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0b111f7bd..350ce6dec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -42,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -87,10 +87,13 @@ #include <mavlink/mavlink_log.h> #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> +#include <external_lgpl/tecs/tecs.h> #include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -115,9 +118,9 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ - int start(); + static int start(); /** * Task status @@ -131,7 +134,6 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ int _global_pos_sub; int _pos_sp_triplet_sub; @@ -144,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + 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 */ @@ -192,6 +195,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -199,6 +203,23 @@ private: float l1_period; float l1_damping; + float time_const; + float time_const_throt; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float climbout_diff; + float heightrate_p; + float speedrate_p; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + float airspeed_min; float airspeed_trim; float airspeed_max; @@ -209,6 +230,7 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; @@ -226,6 +248,23 @@ private: param_t l1_period; param_t l1_damping; + param_t time_const; + param_t time_const_throt; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t climbout_diff; + param_t heightrate_p; + param_t speedrate_p; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -236,6 +275,7 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; @@ -361,7 +401,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), _task_running(false), - _control_task(-1), /* subscriptions */ _global_pos_sub(-1), @@ -376,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), /* states */ @@ -428,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -438,6 +479,23 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); + /* fetch initial parameter values */ parameters_update(); } @@ -485,9 +543,28 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); + + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); @@ -500,6 +577,25 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -561,6 +657,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -621,7 +720,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -733,6 +842,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + /* filter speed and altitude for controller */ + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; + + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -758,6 +875,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -991,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1066,9 +1186,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1084,10 +1204,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1248,20 +1364,74 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int @@ -1295,14 +1465,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (l1_control::g_control != nullptr) errx(1, "already running"); - l1_control::g_control = new FixedwingPositionControl; - - if (l1_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != l1_control::g_control->start()) { - delete l1_control::g_control; - l1_control::g_control = nullptr; + if (OK != FixedwingPositionControl::start()) { err(1, "start failed"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 27039ff51..41c374407 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -83,6 +83,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); /** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + +/** * Negative pitch limit * * The minimum negative pitch the controller will output. @@ -155,6 +166,205 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); /** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +/** + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + +/** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + +/** + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second square) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + +/** + * Height rate P factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); + +/** * Landing slope angle * * @group L1 Control diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..bffeefc1f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Write part of the status message */ _status.altitudeSp = altitudeSp; - _status.altitude = altitude; - _status.altitudeFiltered = altitudeFiltered; + _status.altitude_filtered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ @@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Write part of the status message */ _status.airspeedSp = airspeedSp; - _status.airspeed = airspeed; - _status.airspeedFiltered = airspeedFiltered; + _status.airspeed_filtered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe20..58a1e9f6b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); +PARAM_DEFINE_INT32(MT_ENABLED, 0); /** * Total Energy Rate Control Feedforward @@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); /** * P gain for the airspeed control @@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); /** * Minimal acceleration (air) @@ -287,13 +294,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - -/** * Minimal throttle during takeoff * * @min 0.0f diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp deleted file mode 100644 index b502c3c86..000000000 --- a/src/modules/mavlink/mavlink_commands.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_commands.cpp - * Mavlink commands stream implementation. - * - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#include "mavlink_commands.h" - -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : - _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), - _cmd{}, - _channel(channel), - _cmd_time(0) -{ -} - -void -MavlinkCommandsStream::update(const hrt_abstime t) -{ - struct vehicle_command_s cmd; - - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } - } -} diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a2c900af..17d1babff 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -35,6 +35,7 @@ #include <unistd.h> #include <stdio.h> #include <fcntl.h> +#include <sys/stat.h> #include "mavlink_ftp.h" @@ -190,6 +191,8 @@ void MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); + + hdr->magic = kProtocolMagic; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; @@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; // store the type marker switch (entry.d_type) { case DTYPE_FILE: hdr->data[offset++] = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } break; case DTYPE_DIRECTORY: hdr->data[offset++] = kDirentDir; @@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req) hdr->data[offset++] = kDirentUnknown; break; } + + if (entry.d_type == DTYPE_FILE) { + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + // name too big to fit? + if ((nameLen + offset + 2) > kMaxDataLength) { + break; + } + // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); + strcpy((char *)&hdr->data[offset], buf); //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); - offset += strlen(entry.d_name) + 1; + offset += nameLen + 1; } closedir(dp); @@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create) return kErrNoSession; } + + uint32_t fileSize = 0; + if (!create) { + struct stat st; + if (stat(req->dataAsCString(), &st) != 0) { + return kErrNotFile; + } + fileSize = st.st_size; + } + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; int fd = ::open(req->dataAsCString(), oflag); @@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create) _session_fds[session_index] = fd; hdr->session = session_index; - hdr->size = 0; + if (create) { + hdr->size = 0; + } else { + hdr->size = sizeof(uint32_t); + *((uint32_t*)hdr->data) = fileSize; + } return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 59237a4c4..800c98b69 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -55,6 +55,7 @@ #include <systemlib/err.h> #include "mavlink_messages.h" +#include "mavlink_main.h" class MavlinkFTP { @@ -145,7 +146,7 @@ private: mavlink_message_t msg; msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); + _mavlink->get_channel(), &msg, sequence()+1, rawData()); _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3f64f30ed..e2931ce6d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -78,7 +78,6 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems @@ -91,14 +90,19 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + /** * mavlink app start / stop handling function * @@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; -static uint64_t last_write_success_times[6] = {0}; -static uint64_t last_write_try_times[6] = {0}; - -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) { - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } - - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (instance->should_transmit()) { - last_write_try_times[(unsigned)channel] = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - - if (buf_free < desired) { - /* we don't want to send anything just in half, so return */ - instance->count_txerr(); - instance->count_txerrbytes(desired); - return; - } - } - - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - instance->count_txerr(); - instance->count_txerrbytes(desired); - - } else { - last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; - instance->count_txbytes(desired); - } - } -} - static void usage(void); Mavlink::Mavlink() : @@ -234,8 +131,7 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_manager(nullptr), - _mission_pub(-1), - _mission_result_sub(-1), + _parameters_manager(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, @@ -247,12 +143,16 @@ Mavlink::Mavlink() : _ftp_on(false), _uart_fd(-1), _baudrate(57600), - _datarate(10000), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -263,6 +163,7 @@ Mavlink::Mavlink() : _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, + _send_mutex {}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -485,7 +386,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } @@ -533,10 +439,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - msg.severity = (unsigned char)cmd; + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -562,22 +484,39 @@ void Mavlink::mavlink_update_system(void) _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; @@ -719,6 +658,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -760,8 +702,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ @@ -776,248 +717,188 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::send_message(const mavlink_message_t *msg) +unsigned +Mavlink::get_free_tx_buf() { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return buf_free; } void -Mavlink::handle_message(const mavlink_message_t *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg) { - /* handle packet with mission manager */ - _mission_manager->handle_message(msg); + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } - /* handle packet with parameter component */ - mavlink_pm_message_handler(_channel, msg); + pthread_mutex_lock(&_send_mutex); - if (get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(msg, this); + int buf_free = get_free_tx_buf(); + + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; } -} -int -Mavlink::mavlink_pm_queued_send() -{ - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); } else { - return 1; + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } -} -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; + pthread_mutex_unlock(&_send_mutex); } -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) +void +Mavlink::resend_message(mavlink_message_t *msg) { - return mavlink_pm_send_param(param_for_index(index)); -} + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} + pthread_mutex_lock(&_send_mutex); -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } + int buf_free = get_free_tx_buf(); - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; + _last_write_try_time = hrt_absolute_time(); - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); - int ret; + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - send_message(&tx_msg); - return OK; + pthread_mutex_unlock(&_send_mutex); } -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +void +Mavlink::handle_message(const mavlink_message_t *msg) { - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - mavlink_param_request_list_t req; - mavlink_msg_param_request_list_decode(msg, &req); - - if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - send_statustext_info("[pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid - && ((mavlink_param_set.target_component == mavlink_system.compid) - || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - send_statustext_info(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid - && ((mavlink_param_request_read.target_component == mavlink_system.compid) - || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } + /* handle packet with parameter component */ + _parameters_manager->handle_message(msg); - } break; + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); } } -int +void Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAV_SEVERITY_INFO, string); } -int +void Mavlink::send_statustext_critical(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAV_SEVERITY_CRITICAL, string); } -int +void Mavlink::send_statustext_emergency(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAV_SEVERITY_EMERGENCY, string); } -int -Mavlink::send_statustext(unsigned severity, const char *string) +void +Mavlink::send_statustext(unsigned char severity, const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - - /* Map severity */ - switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; - } - - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); - return OK; - - } else { - return ERROR; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1040,11 +921,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1075,10 +962,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); LL_APPEND(_streams, stream); return OK; @@ -1092,6 +977,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) } void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + +void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { /* orb subscription must be done from the main thread, @@ -1251,7 +1161,27 @@ Mavlink::pass_message(const mavlink_message_t *msg) float Mavlink::get_rate_mult() { - return _datarate / 1000.0f; + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); } int @@ -1389,6 +1319,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); @@ -1398,7 +1331,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1418,12 +1351,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - - /* create mission manager */ - _mission_manager = new MavlinkMissionManager(this); - _mission_manager->set_verbose(_verbose); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1434,34 +1361,50 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = get_rate_mult(); + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 8.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f); + configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); break; @@ -1469,13 +1412,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1488,6 +1426,8 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -1498,9 +1438,6 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1526,20 +1463,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - _mission_manager->eventloop(); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - send_statustext(msg.severity, msg.text); - } - } - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { @@ -1584,7 +1507,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + resend_message(&msg); } } @@ -1599,14 +1522,13 @@ Mavlink::task_main(int argc, char *argv[]) _bytes_txerr = 0; _bytes_rx = 0; } + _bytes_timestamp = t; } perf_end(_loop_perf); } - delete _mission_manager; - delete _subscribe_to_stream; _subscribe_to_stream = nullptr; @@ -1764,10 +1686,12 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 260a7a224..d4d15d81f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -58,7 +58,7 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" #include "mavlink_mission.h" - +#include "mavlink_parameters.h" class Mavlink { @@ -162,7 +162,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + void send_message(const uint8_t msgid, const void *msg); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -190,29 +195,33 @@ public: * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_info(const char *string); + void send_statustext_info(const char *string); /** * Send a status text with loglevel CRITICAL * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_critical(const char *string); + void send_statustext_critical(const char *string); /** * Send a status text with loglevel EMERGENCY * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_emergency(const char *string); + void send_statustext_emergency(const char *string); /** - * Send a status text with loglevel + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level */ - int send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned char severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -254,6 +263,8 @@ public: */ struct telemetry_status_s& get_rx_status() { return _rstatus; } + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + protected: Mavlink *next; @@ -277,9 +288,8 @@ private: MavlinkStream *_streams; MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; - orb_advert_t _mission_pub; - int _mission_result_sub; MAVLINK_MODE _mode; mavlink_channel_t _channel; @@ -295,7 +305,9 @@ private: bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -310,6 +322,8 @@ private: float _subscribe_to_stream_rate; bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; unsigned _bytes_tx; unsigned _bytes_txerr; @@ -331,6 +345,7 @@ private: mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; bool _param_initialized; param_t _param_system_id; @@ -342,52 +357,27 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); + void mavlink_update_system(); - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); /** - * Send one parameter identified by name. + * Get the free space in the transmit buffer * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. + * @return free space in the UART TX buffer */ - int mavlink_pm_send_param_for_name(const char *name); + unsigned get_free_tx_buf(); - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); /** - * Start sending the parameter queue. + * Adjust the stream rates based on the current rate * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease */ - void mavlink_pm_start_queued_send(); - - void mavlink_update_system(); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - int configure_stream(const char *stream_name, const float rate); + void adjust_stream_rates(const float multiplier); int message_buffer_init(int size); @@ -403,6 +393,11 @@ private: void pass_message(const mavlink_message_t *msg); + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..c10be77b5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include <stdio.h> + #include <commander/px4_custom_mode.h> #include <lib/geo/geo.h> - #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -72,11 +72,11 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_range_finder.h> - #include <systemlib/err.h> +#include <mavlink/mavlink_log.h> #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -249,61 +249,184 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - explicit MavlinkStreamHeartbeat() : MavlinkStream(), - status_sub(nullptr), - pos_sp_triplet_sub(nullptr) + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_heartbeat_t msg; - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = mavlink_system.type; + msg.autopilot = MAV_AUTOPILOT_PX4; + msg.mavlink_version = 3; + + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + msg.severity = logmsg.severity; + strncpy(msg.text, logmsg.text, sizeof(msg.text)); + + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + } + } + } +}; + +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t msg; + + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -313,7 +436,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -323,47 +446,50 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamSysStatus(); + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), - status_sub(nullptr) + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + if (_status_sub->update(&status)) { + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; @@ -387,78 +513,89 @@ public: return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHighresIMU(); + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); } } }; @@ -482,14 +619,19 @@ public: return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitude(); + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); @@ -497,25 +639,27 @@ private: protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - att_time(0) + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); } } }; @@ -539,44 +683,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamAttitudeQuaternion(); + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - att_time(0) + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); } } }; @@ -601,54 +748,50 @@ public: return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamVFRHUD(); + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -657,24 +800,23 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = pos.alt; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); } } }; @@ -698,46 +840,49 @@ public: return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGPSRawInt(); + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - gps_time(0) + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), + _gps_time(0) {} - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_used); + if (_gps_sub->update(&_gps_time, &gps)) { + mavlink_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } } }; @@ -761,55 +906,57 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionInt(); + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *home_sub; - uint64_t home_time; + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - void send(const hrt_abstime t) { struct vehicle_global_position_s pos; struct home_position_s home; - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); + bool updated = _pos_sub->update(&_pos_time, &pos); + updated |= _home_sub->update(&_home_time, &home); if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } } }; @@ -833,49 +980,51 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamLocalPositionNED(); + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } } }; - class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: @@ -894,43 +1043,46 @@ public: return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamViconPositionEstimate(); + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; /* do not allow top copying this class */ MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - void send(const hrt_abstime t) { struct vehicle_vicon_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } } }; @@ -954,45 +1106,49 @@ public: return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSGlobalOrigin(); + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() + { + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *home_sub; + MavlinkOrbSubscription *_home_sub; /* do not allow top copying this class */ MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - void send(const hrt_abstime t) { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ - if (home_sub->is_published()) { + if (_home_sub->is_published()) { struct home_position_s home; - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); } } } }; + template <int N> class MavlinkStreamServoOutputRaw : public MavlinkStream { @@ -1024,26 +1180,28 @@ public: } } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamServoOutputRaw<N>(); + return new MavlinkStreamServoOutputRaw<N>(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), @@ -1052,25 +1210,28 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } void send(const hrt_abstime t) { struct actuator_outputs_s act; - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); } } }; @@ -1094,51 +1255,49 @@ public: return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHILControls(); + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(0) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); + bool updated = _act_sub->update(&_act_time, &act); + updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); + updated |= _status_sub->update(&_status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ @@ -1206,67 +1365,79 @@ protected: } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamPositionTargetGlobalInt(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamGlobalPositionSetpointInt(); + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_position_target_global_int_t msg{}; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1282,165 +1453,124 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamLocalPositionSetpoint(); + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; + MavlinkOrbSubscription *_pos_sp_sub; + uint64_t _pos_sp_time; /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_position_target_local_ned_t msg{}; + + msg.time_boot_ms = pos_sp.timestamp / 1000; + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRollPitchYawThrustSetpoint(); + return new MavlinkStreamAttitudeTarget(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } + mavlink_attitude_target_t msg{}; - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } + msg.time_boot_ms = att_sp.timestamp / 1000; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } + msg.thrust = att_sp.thrust; -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -1464,77 +1594,84 @@ public: return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRCChannelsRaw(); + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() + { + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; /* do not allow top copying this class */ MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) {} - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - void send(const hrt_abstime t) { struct rc_input_values rc; - if (rc_sub->update(&rc_time, &rc)) { + if (_rc_sub->update(&_rc_time, &rc)) { const unsigned port_width = 8; - // Deprecated message (but still needed for compatibility!) + /* deprecated message (but still needed for compatibility!) */ for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); } - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } }; @@ -1558,42 +1695,45 @@ public: return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamManualControl(); + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) {} - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - void send(const hrt_abstime t) { struct manual_control_setpoint_s manual; - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } } }; @@ -1617,42 +1757,47 @@ public: return MAVLINK_MSG_ID_OPTICAL_FLOW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(); + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() + { + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; /* do not allow top copying this class */ MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) {} - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - void send(const hrt_abstime t) { struct optical_flow_s flow; - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); } } }; @@ -1675,56 +1820,64 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitudeControls(); + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() + { + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; /* do not allow top copying this class */ MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - void send(const hrt_abstime t) { struct actuator_controls_s att_ctrl; - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamNamedValueFloat : public MavlinkStream { public: @@ -1743,46 +1896,49 @@ public: return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamNamedValueFloat(); + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) {} - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - void send(const hrt_abstime t) { struct debug_key_value_s debug; - if (debug_sub->update(&debug_time, &debug)) { + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamCameraCapture : public MavlinkStream { public: @@ -1801,46 +1957,53 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamCameraCapture(); + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), - status_sub(nullptr) + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(&status); + (void)_status_sub->update(&status); - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_command_long_t msg; - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } }; + class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -1859,50 +2022,58 @@ public: return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamDistanceSensor(); + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) {} - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - void send(const hrt_abstime t) { struct range_finder_report range; - if (range_sub->update(&range_time, &range)) { + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; - uint8_t type; + msg.time_boot_ms = range.timestamp / 1000; switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } } }; @@ -1910,6 +2081,8 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), @@ -1918,16 +2091,16 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), @@ -1935,6 +2108,5 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e42..7e4416609 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 068774c47..7a761588a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -59,8 +59,7 @@ static const int ERROR = -1; -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : - _mavlink(mavlink), +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(-1), - _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _slow_rate_limiter(_interval / 10.0f), _verbose(false), - _channel(mavlink->get_channel()), _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager() close(_mission_result_sub); } +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void MavlinkMissionManager::init_offboard_mission() { @@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) { - mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -175,13 +185,11 @@ void MavlinkMissionManager::send_mission_current(uint16_t seq) { if (seq < _count) { - mavlink_message_t msg; mavlink_mission_current_t wpc; wpc.seq = seq; - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = sysid; wpc.target_component = compid; wpc.count = _count; - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mavlink_mission_item_t wp; format_mavlink_mission_item(&mission_item, &wp); - mavlink_message_t msg; wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (seq < _max_count) { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 void MavlinkMissionManager::send_mission_item_reached(uint16_t seq) { - mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } void -MavlinkMissionManager::eventloop() +MavlinkMissionManager::send(const hrt_abstime now) { - hrt_abstime now = hrt_absolute_time(); + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b792b9aaf..8ff27f87d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -42,11 +42,12 @@ #pragma once +#include <uORB/uORB.h> + #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" -#include <uORB/uORB.h> +#include "mavlink_stream.h" -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -class Mavlink; - -class MavlinkMissionManager { +class MavlinkMissionManager : public MavlinkStream { public: - MavlinkMissionManager(Mavlink *parent); - ~MavlinkMissionManager(); + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + uint8_t _comp_id; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + void init_offboard_mission(); int update_active_mission(int dataman_id, unsigned count, int seq); - void send_message(mavlink_message_t *msg); - /** * @brief Sends an waypoint ack message */ @@ -111,10 +167,6 @@ public: */ void send_mission_item_reached(uint16_t seq); - void eventloop(); - - void handle_message(const mavlink_message_t *msg); - void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); @@ -139,43 +191,8 @@ public: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - void set_verbose(bool v) { _verbose = v; } - -private: - Mavlink* _mavlink; - - enum MAVLINK_WPM_STATES _state; ///< Current state - - uint64_t _time_last_recv; - uint64_t _time_last_sent; - - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximum number of mission items - - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission - - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; - - MavlinkRateLimiter _slow_rate_limiter; - - bool _verbose; - - mavlink_channel_t _channel; - uint8_t _comp_id; +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); - /* do not allow copying this class */ - MavlinkMissionManager(const MavlinkMissionManager&); - MavlinkMissionManager operator=(const MavlinkMissionManager&); + void send(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 000000000..cd5f53d88 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> + +#include "mavlink_parameters.h" +#include "mavlink_main.h" + +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= (int) param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_parameters.h index abdba34b9..5576e6b84 100644 --- a/src/modules/mavlink/mavlink_commands.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,34 +32,84 @@ ****************************************************************************/ /** - * @file mavlink_commands.h - * Mavlink commands stream definition. + * @file mavlink_parameters.h + * Mavlink parameters manager definition. * * @author Anton Babushkin <anton.babushkin@me.com> */ -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ +#pragma once -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_command.h> +#include <systemlib/param/param.h> -class Mavlink; -class MavlinkCommansStream; +#include "mavlink_bridge_header.h" +#include "mavlink_stream.h" -#include "mavlink_main.h" - -class MavlinkCommandsStream +class MavlinkParametersManager : public MavlinkStream { +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void start_send_all(); + private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; + int _send_all_index; -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); -#endif /* MAVLINK_COMMANDS_H_ */ + void send(const hrt_abstime t); + + void send_param(param_t param); +}; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17..d3b3e1cde 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b7cddf523..dfc5ddc91 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -150,18 +150,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); break; - case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL: - handle_message_local_ned_position_setpoint_external(msg); + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); break; - - case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL: - handle_message_attitude_setpoint_external(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -251,7 +246,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } @@ -372,53 +368,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* Convert values * 1000 back */ - //XXX: convert to quaternion - //offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - //offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - //offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - //offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } - } -} - -void -MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg) -{ - mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external; - mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system || - local_ned_position_setpoint_external.target_system == 0) && - (mavlink_system.compid == local_ned_position_setpoint_external.target_component || - local_ned_position_setpoint_external.target_component == 0)) { + if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - switch (local_ned_position_setpoint_external.coordinate_frame) { + switch (set_position_target_local_ned.coordinate_frame) { case MAV_FRAME_LOCAL_NED: offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; break; @@ -435,16 +400,16 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes /* invalid setpoint, avoid publishing */ return; } - offboard_control_sp.position[0] = local_ned_position_setpoint_external.x; - offboard_control_sp.position[1] = local_ned_position_setpoint_external.y; - offboard_control_sp.position[2] = local_ned_position_setpoint_external.z; - offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx; - offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy; - offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz; - offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx; - offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; - offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; - offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + offboard_control_sp.position[0] = set_position_target_local_ned.x; + offboard_control_sp.position[1] = set_position_target_local_ned.y; + offboard_control_sp.position[2] = set_position_target_local_ned.z; + offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; + offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; + offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; + offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; + offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; + offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); /* If we are in force control mode, for now set offboard mode to force control */ if (offboard_control_sp.isForceSetpoint) { @@ -454,7 +419,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } @@ -540,32 +505,32 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } void -MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg) +MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) { - mavlink_attitude_setpoint_external_t attitude_setpoint_external; - mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external); + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == attitude_setpoint_external.target_system || - attitude_setpoint_external.target_system == 0) && - (mavlink_system.compid == attitude_setpoint_external.target_component || - attitude_setpoint_external.target_component == 0)) { + if ((mavlink_system.sysid == set_attitude_target.target_system || + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { for (int i = 0; i < 4; i++) { - offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; + offboard_control_sp.attitude[i] = set_attitude_target.q[i]; } - offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate; - offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate; - offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate; + offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; + offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; for (int i = 0; i < 3; i++) { offboard_control_sp.ignore &= ~(1 << (i + 9)); - offboard_control_sp.ignore |= (attitude_setpoint_external.type_mask & (1 << i)) << 9; + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << 9; } offboard_control_sp.ignore &= ~(1 << 10); - offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -590,14 +555,14 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if ignore bit is not set */ - if (!(attitude_setpoint_external.type_mask & (1 << 7))) { + if (!(set_attitude_target.type_mask & (1 << 7))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); - mavlink_quaternion_to_euler(attitude_setpoint_external.q, + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - att_sp.thrust = attitude_setpoint_external.thrust; + att_sp.thrust = set_attitude_target.thrust; att_sp.q_d_valid = true; - memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d)); + memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { @@ -607,13 +572,13 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Publish attitude rate setpoint if ignore bit are not set */ ///XXX add support for ignoring individual axes - if (!(attitude_setpoint_external.type_mask & (0b111))) { + if (!(set_attitude_target.type_mask & (0b111))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); - rates_sp.roll = attitude_setpoint_external.body_roll_rate; - rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; - rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; - rates_sp.thrust = attitude_setpoint_external.thrust; + rates_sp.roll = set_attitude_target.body_roll_rate; + rates_sp.pitch = set_attitude_target.body_pitch_rate; + rates_sp.yaw = set_attitude_target.body_yaw_rate; + rates_sp.thrust = set_attitude_target.thrust; if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); @@ -626,7 +591,6 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms } } } - void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a2ed4264f..e3b635dd2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -114,8 +114,8 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); - void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); - void handle_message_attitude_setpoint_external(mavlink_message_t *msg); + void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206db..5b9e45d3e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -65,26 +65,27 @@ MavlinkStream::set_interval(const unsigned int interval) } /** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - -/** * Update subscriptions and send message if necessary */ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 20e1c7c44..ef22dc443 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,31 +46,50 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); - void set_channel(mavlink_channel_t channel); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } /** * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1986ae3c8..91fdd6154 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,11 +40,11 @@ SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ + mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink 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 9b0f69226..ecc40428d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -105,6 +105,7 @@ public: int start(); private: + const float alt_ctl_dz = 0.2f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ @@ -184,6 +185,8 @@ private: math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -217,6 +220,16 @@ private: void reset_alt_sp(); /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + /** * Select between barometric and global (AMSL) altitudes */ void select_alt(bool global); @@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -508,12 +528,126 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode.flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual.x; + _sp_move_rate(1) = _manual.y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet.current.vx; + _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet.current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +void MulticopterPositionControl::task_main() { warnx("started"); @@ -551,13 +685,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - - math::Vector<3> sp_move_rate; - sp_move_rate.zero(); - - float yaw_sp_move_rate; - math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -616,138 +743,17 @@ MulticopterPositionControl::task_main() _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; - sp_move_rate.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); - } - - if (_control_mode.flag_control_position_enabled) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.x; - sp_move_rate(1) = _manual.y; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } + control_manual(dt); } else if (_control_mode.flag_control_offboard_enabled) { - /* Offboard control */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - - if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { - - _pos_sp(0) = _pos_sp_triplet.current.x; - _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _pos_sp_triplet.current.vx; - sp_move_rate(1) = _pos_sp_triplet.current.vy; - yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; - _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; - } - - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } - - } else { - reset_pos_sp(); - reset_alt_sp(); - } + /* offboard control */ + control_offboard(dt); } else { /* AUTO */ @@ -782,6 +788,7 @@ MulticopterPositionControl::task_main() } /* fill local position setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); @@ -821,7 +828,7 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; @@ -901,7 +908,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..c0e37a3ed 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -273,6 +273,10 @@ Mission::check_dist_1wp() if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } return true; } else { @@ -314,30 +318,36 @@ Mission::set_mission_items() /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* get home distance state */ + bool home_dist_ok = check_dist_1wp(); + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = !home_dist_ok; + /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { - /* no mission available, switch to loiter */ + /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: mission finished"); - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: no mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + } else if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; @@ -397,7 +407,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; @@ -483,7 +493,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR waypoint could not be read"); + "ERROR waypoint could not be read"); return false; } @@ -502,7 +512,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -511,8 +521,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -526,7 +536,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP is cycling, giving up"); + "ERROR DO JUMP is cycling, giving up"); return false; } diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 881caa24e..d15e1e771 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * waypoint is more distant than MIS_DIS_1WP from the current position. * * @min 0 - * @max 250 + * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 31861c3fc..6c4b49452 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; - log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 853a3811f..fb7609435 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -333,13 +333,11 @@ struct log_GS1B_s { #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; - float altitude; float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749e..64317a18a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce8..b60584608 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,7 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include <stdbool.h> diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a1932180..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,7 +60,7 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ }; struct position_setpoint_s @@ -71,9 +71,9 @@ struct position_setpoint_s float y; /**< local position setpoint in m in NED */ float z; /**< local position setpoint in m in NED */ bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m in NED */ - float vy; /**< local velocity setpoint in m in NED */ - float vz; /**< local velocity setpoint in m in NED */ + float vx; /**< local velocity setpoint in m/s in NED */ + float vy; /**< local velocity setpoint in m/s in NED */ + float vz; /**< local velocity setpoint in m/s in NED */ bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 33055018c..ddca19d61 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -51,11 +51,13 @@ */ typedef enum { - TECS_MODE_NORMAL, + TECS_MODE_NORMAL = 0, TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF, TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM + TECS_MODE_LAND_THROTTLELIM, + TECS_MODE_BAD_DESCENT, + TECS_MODE_CLIMBOUT } tecs_mode; /** @@ -65,14 +67,12 @@ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ float altitudeSp; - float altitude; - float altitudeFiltered; + float altitude_filtered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; - float airspeedFiltered; + float airspeed_filtered; float airspeedDerivativeSp; float airspeedDerivative; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e32529cb4..25137c1c6 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -70,8 +70,8 @@ struct vehicle_global_position_s { float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; - float epv; + float eph; /**< Standard deviation of position estimate horizontally */ + float epv; /**< Standard deviation of position vertically */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index 8988a0330..6766bb58a 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -50,11 +50,12 @@ */ struct vehicle_local_position_setpoint_s { + uint64_t timestamp; /**< timestamp of the setpoint */ float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame to go to */ +}; /**< Local position in NED frame */ /** * @} diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 7a40ac636..d9220fe14 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -186,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; @@ -228,6 +228,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1ef6f0cfa..3865f2468 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # # Invoke DSDL compiler -# TODO: Add make target for this, or invoke dsdlc manually. -# The second option assumes that the generated headers shall be saved -# under the version control, which may be undesirable. -# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 27e77e9c5..4535b6d6a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -39,6 +39,8 @@ #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> +#include <systemlib/board_serial.h> +#include <version/version.h> #include <arch/board/board.h> #include <arch/chip/chip.h> @@ -173,6 +175,41 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return OK; } +void UavcanNode::fill_node_info() +{ + /* software version */ + uavcan::protocol::SoftwareVersion swver; + + // Extracting the first 8 hex digits of FW_GIT and converting them to int + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + + warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + + _node.setSoftwareVersion(swver); + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + + _node.setHardwareVersion(hwver); +} + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -183,6 +220,13 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + _node.setName("org.pixhawk.pixhawk"); + + _node.setNodeID(node_id); + + fill_node_info(); + + /* initializing the bridges UAVCAN <--> uORB */ ret = _esc_controller.init(); if (ret < 0) return ret; @@ -191,20 +235,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - uavcan::protocol::SoftwareVersion swver; - swver.major = 12; // TODO fill version info - swver.minor = 34; - _node.setSoftwareVersion(swver); - - uavcan::protocol::HardwareVersion hwver; - hwver.major = 42; // TODO fill version info - hwver.minor = 42; - _node.setHardwareVersion(hwver); - - _node.setName("org.pixhawk"); // Huh? - - _node.setNodeID(node_id); - return _node.start(); } @@ -223,7 +253,6 @@ int UavcanNode::run() // XXX figure out the output count _output_count = 2; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); actuator_outputs_s outputs; @@ -243,21 +272,23 @@ int UavcanNode::run() _node.setStatusOk(); - while (!_task_should_exit) { + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds_num = 0; + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -271,7 +302,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { @@ -282,12 +313,7 @@ int UavcanNode::run() } } - if (!controls_updated) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } - - //can we mix? + // can we mix? if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -387,7 +413,8 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; + // the first fd used by CAN + _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -493,8 +520,8 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } /* diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 443525379..05b66fd7b 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: static UavcanNode* instance() { return _instance; } private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); int run(); diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index b22b446da..e2fa0ff80 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1400 |