diff options
Diffstat (limited to 'src')
26 files changed, 1085 insertions, 181 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 771f2128f..524151c90 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -76,8 +76,8 @@ #include <drivers/airspeed/airspeed.h> -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : - I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : + I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c27b1bcd8..186602eda 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval); + Airspeed(int bus, int address, unsigned conversion_interval, const char* path); virtual ~Airspeed(); virtual int init(); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 8bbef5cfa..b6e80ce1d 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -77,6 +77,7 @@ /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define ETS_PATH "/dev/ets_airspeed" /* Register address */ #define READ_CMD 0x07 /* Read the data */ @@ -93,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); protected: @@ -112,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f..06d89abf7 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -50,6 +50,7 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -89,8 +90,10 @@ /* I2C bus address is 1010001x */ #define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#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 */ +#define PATH_MS5525 "/dev/ms5525" /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ @@ -101,7 +104,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); protected: @@ -120,8 +123,8 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -304,7 +307,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ if (g_dev == nullptr) @@ -313,7 +316,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) @@ -386,7 +389,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -411,7 +414,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index ec5f77d74..705e98eea 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if (Motor[i].MaxPWM == 250) { + if ((Motor[i].MaxPWM & 252) == 248) { Motor[i].Version = BLCTRL_NEW; } else { diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bf80c9cff..ac75682c4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1353,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { } diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 63a8981aa..45d7957f1 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,28 +45,46 @@ #include <systemlib/param/param.h> /* - * Launch detection parameters, accessible via MAVLink + * Catapult launch detection parameters, accessible via MAVLink * */ -/* Catapult Launch detection */ - -// @DisplayName Switch to enable launchdetection -// @Description if set to 1 launchdetection is enabled -// @Range 0 or 1 +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); -// @DisplayName Catapult Accelerometer Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0 +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); -// @DisplayName Catapult Time Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0, in seconds +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); -// @DisplayName Throttle setting while detecting the launch -// @Description The throttle is set to this value while the system is waiting for the takeoff -// @Range 0 to 1 +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b8573..6d14472f3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0; static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static int parachute_enabled = 0; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + //XXX: to enable the parachute, a param needs to be set + //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); /* welcome user */ warnx("starting"); @@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); } + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + param_get(_param_enable_parachute, ¶chute_enabled); } orb_check(sp_man_sub, &updated); @@ -1152,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { + if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { @@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index d3155f7bf..80ca68f21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,7 +48,39 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Empty cell voltage. + * + * Defines the voltage where a single cell of the battery is considered empty. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); + +/** + * Full cell voltage. + * + * Defines the voltage where a single cell of the battery is considered full. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); + +/** + * Number of cells. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); + +/** + * Battery capacity. + * + * Defines the capacity of the attached battery. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); 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 512ca7b8a..62a340e90 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 @@ -40,12 +40,10 @@ */ #include <nuttx/config.h> - #include <systemlib/param/param.h> /* * Controller parameters, accessible via MAVLink - * */ /** @@ -119,58 +117,268 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); - +/** + * Controller roll limit + * + * The maximum roll the controller will output. + * + * @unit degrees + * @min 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); +/** + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that + * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +/** + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide + * some aerodynamic drag from a turning prop to improve the descent rate. + * + * For aircraft with internal combustion engine this parameter should be set + * for desired idle rpm. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); - -PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); - +/** + * Throttle limit value before flare + * + * This throttle value will be set as throttle limit at FW_LND_TLALT, + * before arcraft will flare. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.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 L1 Control + */ 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 L1 Control + */ +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 L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.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 L1 Control + */ 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 L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second^2) + * 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 L1 Control + */ 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 L1 Control + */ 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 L1 Control + */ 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 L1 Control + */ 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 L1 Control + */ 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 L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - +/** + * Height rate P factor + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); +/** + * Landing slope angle + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/** + * Landing slope length + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); + +/** + * + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/** + * Landing flare altitude (relative) + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); + +/** + * Landing throttle limit altitude (relative) + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); + +/** + * Landing heading hold horizontal distance + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..ade4469c5 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -76,8 +76,20 @@ #include <uORB/topics/mission_result.h> /* define MAVLink specific parameters */ +/** + * MAVLink system ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +/** + * MAVLink component ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +/** + * MAVLink type + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a371a499e..1dbe56495 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -351,7 +351,7 @@ handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub == 0) { + if (telemetry_status_pub <= 0) { telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index db5e2e9bb..24226880e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,11 +53,9 @@ #include <stdlib.h> #include <string.h> #include <unistd.h> -#include <fcntl.h> #include <errno.h> #include <math.h> #include <poll.h> -#include <time.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> @@ -71,7 +69,6 @@ #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <systemlib/pid/pid.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> @@ -84,9 +81,9 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f -#define RATES_I_LIMIT 0.5f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl { @@ -135,15 +132,13 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ - math::Matrix<3, 3> _R; /**< rotation matrix for current state */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ - math::Matrix<3, 3> I; /**< identity matrix */ + math::Matrix<3, 3> _I; /**< identity matrix */ bool _reset_yaw_sp; /**< reset yaw setpoint flag */ @@ -262,7 +257,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -276,15 +271,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_i.zero(); _params.rate_d.zero(); - _R_sp.identity(); - _R.identity(); _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); - I.identity(); + _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); @@ -535,16 +528,18 @@ MulticopterAttitudeControl::control_attitude(float dt) _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - _R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0][0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -561,23 +556,24 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* rotation matrix for current state */ - _R.set(_v_att.R); + math::Matrix<3, 3> R; + R.set(_v_att.R); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); - math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ - float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; @@ -600,15 +596,15 @@ MulticopterAttitudeControl::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ - R_rp = _R; + R_rp = R; } - /* R_rp and _R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; @@ -616,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; - q.from_dcm(_R.transposed() * _R_sp); + q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); @@ -658,7 +654,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f) { + if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -695,9 +691,6 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ - orb_set_interval(_v_att_sub, 5); - /* initialize parameters cache */ parameters_update(); @@ -767,10 +760,12 @@ MulticopterAttitudeControl::task_main() } } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bb..488107d58 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,16 +41,135 @@ #include <systemlib/param/param.h> +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); 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 25d34c872..b06655385 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -51,7 +51,6 @@ #include <errno.h> #include <math.h> #include <poll.h> -#include <time.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> @@ -68,7 +67,6 @@ #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <systemlib/pid/pid.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <lib/geo/geo.h> diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 9eb56545d..0082a5e6a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -39,20 +39,164 @@ #include <systemlib/param/param.h> -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); + +/** + * Maximum tilt + * + * Limits maximum tilt in AUTO and EASY modes. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); + +/** + * Landing descend rate + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); + +/** + * Maximum landing tilt + * + * Limits maximum tilt on landing. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 20dd1fe2f..5831a0ca9 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -45,11 +45,17 @@ #include <systemlib/param/param.h> /* - * geofence parameters, accessible via MAVLink - * + * Geofence parameters, accessible via MAVLink */ -// @DisplayName Switch to enable geofence -// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present -// @Range 0 or 1 +/** + * Enable geofence. + * + * Set to 1 to enable geofence. + * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * + * @min 0 + * @max 1 + * @group Geofence + */ PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d7f6fd16a..0ed12d106 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -306,6 +306,12 @@ private: void start_land_home(); /** + * Fork for state transitions + */ + void request_loiter_or_ready(); + void request_mission_if_available(); + + /** * Guards offboard mission */ bool offboard_mission_available(unsigned relative_index); @@ -699,24 +705,17 @@ Navigator::task_main() } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); + request_mission_if_available(); stick_mode = true; } } @@ -733,17 +732,11 @@ Navigator::task_main() break; case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); break; case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); break; case NAV_STATE_RTL: @@ -770,12 +763,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_mission_if_available(); } } } @@ -1071,7 +1059,7 @@ Navigator::start_loiter() float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { + if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); @@ -1080,9 +1068,8 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } - + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; @@ -1406,6 +1393,28 @@ Navigator::set_rtl_item() } void +Navigator::request_loiter_or_ready() +{ + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } +} + +void +Navigator::request_mission_if_available() +{ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + request_loiter_or_ready(); + } +} + +void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; @@ -1574,13 +1583,7 @@ Navigator::on_mission_item_reached() /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - if (_vstatus.condition_landed) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1ba159a8e..9ef359c6d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,14 +50,91 @@ /* * Navigator parameters, accessible via MAVLink - * */ +/** + * Minimum altitude (fixed wing only) + * + * Minimum altitude above home for LOITER. + * + * @unit meters + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); + +/** + * Waypoint acceptance radius + * + * Default value of acceptance radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); + +/** + * Loiter radius (fixed wing only) + * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Enable onboard mission + * + * @group Navigation + */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT + +/** + * Take-off altitude + * + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); + +/** + * Landing altitude + * + * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); + +/** + * Return-To-Launch altitude + * + * Minimum altitude above home position for going home in RTL mode. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); + +/** + * Return-To-Launch delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); + +/** + * Enable parachute deployment + * + * @group Navigation + */ +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index bf4f7ae97..ad363efe0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -42,14 +42,11 @@ #include <stdio.h> #include <stdbool.h> #include <fcntl.h> -#include <float.h> #include <string.h> #include <nuttx/config.h> #include <nuttx/sched.h> #include <sys/prctl.h> #include <termios.h> -#include <errno.h> -#include <limits.h> #include <math.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec..6a4429461 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,7 +179,7 @@ mixer_tick(void) ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3c218e21f..68e6a7469 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -82,6 +82,7 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/esc_status.h> +#include <uORB/topics/telemetry_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; + struct telemetry_status_s telemetry; } buf; memset(&buf, 0, sizeof(buf)); @@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; + int telemetry_sub; } subs; /* log message buffer: header + body */ @@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; + struct log_TELE_s log_TELE; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- TELEMETRY STATUS --- */ + subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + fds[fdsc_count].fd = subs.telemetry_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- TELEMETRY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index db87e3a6a..16bfc355d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -264,6 +264,18 @@ struct log_DIST_s { uint8_t flags; }; +/* --- TELE - TELEMETRY STATUS --- */ +#define LOG_TELE_MSG 22 +struct log_TELE_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a..288c6e00a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -42,13 +42,10 @@ */ #include <nuttx/config.h> - #include <systemlib/param/param.h> /** - * Gyro X offset - * - * This is an X-axis offset for the gyro. Adjust it according to the calibration data. + * Gyro X-axis offset * * @min -10.0 * @max 10.0 @@ -57,7 +54,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset + * Gyro Y-axis offset * * @min -10.0 * @max 10.0 @@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset + * Gyro Z-axis offset * * @min -5.0 * @max 5.0 @@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); /** - * Gyro X scaling - * - * X-axis scaling. + * Gyro X-axis scaling factor * * @min -1.5 * @max 1.5 @@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); /** - * Gyro Y scaling - * - * Y-axis scaling. + * Gyro Y-axis scaling factor * * @min -1.5 * @max 1.5 @@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); /** - * Gyro Z scaling - * - * Z-axis scaling. + * Gyro Z-axis scaling factor * * @min -1.5 * @max 1.5 @@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + /** - * Magnetometer X offset - * - * This is an X-axis offset for the magnetometer. + * Magnetometer X-axis offset * * @min -500.0 * @max 500.0 @@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); /** - * Magnetometer Y offset - * - * This is an Y-axis offset for the magnetometer. + * Magnetometer Y-axis offset * * @min -500.0 * @max 500.0 @@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); /** - * Magnetometer Z offset - * - * This is an Z-axis offset for the magnetometer. + * Magnetometer Z-axis offset * * @min -500.0 * @max 500.0 @@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +/** + * Differential pressure sensor offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog enabled + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + /** * RC Channel 1 Minimum * @@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif -PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +/** + * DSM binding trigger. + * + * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + * + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); + + +/** + * Scaling factor for battery voltage sensor on PX4IO. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/** + * Scaling factor for battery voltage sensor on FMU v2. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ -/* PX4IOAR: 0.00838095238 */ -/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +/** + * Scaling factor for battery voltage sensor on FMU v1. + * + * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 + * FMUv1 with PX4IO: 0.00459340659 + * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif + +/** + * Scaling factor for battery current sensor. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ + /** * Roll control channel mapping. * @@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Assist switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); + +/** + * Mission switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Flaps channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +/** + * Auxiliary switch 1 channel mapping. + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); + +/** + * Auxiliary switch 2 channel mapping. + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ +/** + * Auxiliary switch 3 channel mapping. + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + + +/** + * Roll scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); + +/** + * Pitch scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); + +/** + * Yaw scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); -PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ + +/** + * Failsafe channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_CH, 0); + +/** + * Failsafe channel mode. + * + * 0 = too low means signal loss, + * 1 = too high means signal loss + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); + +/** + * Failsafe channel PWM threshold. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 75be090f8..cb35a2541 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,8 +40,23 @@ #include <nuttx/config.h> #include <systemlib/param/param.h> -// Auto-start script with index #n +/** + * Auto-start script index. + * + * Defines the auto-start script used to bootstrap the system. + * + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); -// Automatically configure default values +/** + * Automatically configure default values. + * + * Set to 1 to set platform-specific parameters to their default + * values on next system startup. + * + * @min 0 + * @max 1 + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc..5192d4d58 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ @@ -73,4 +73,4 @@ struct telemetry_status_s { ORB_DECLARE(telemetry_status); -#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file +#endif /* TOPIC_TELEMETRY_STATUS_H */ |