diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 156711c26d..b718f421f5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -17,9 +17,9 @@ then param set FW_T_TIME_CONST 5 param set PE_VELNE_NOISE 0.3 - param set PE_VELD_NOISE 0.5 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 0.5 + param set PE_POSD_NOISE 1.0 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0002 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index fa3653e0d5..a5c326ebc6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -5,9 +5,9 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 + param set PE_POSD_NOISE 1.25 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 844d540bf0..c2340a7b16 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -34,9 +34,9 @@ then param set FW_RR_P 0.02 param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.3 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 + param set PE_POSD_NOISE 1.25 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0fb6e117cc..0a4887a639 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -457,13 +457,13 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_F "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 5000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_F "-r 1000" + set MAVLINK_F "-r 5000" fi fi @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x fi if param compare SYS_COMPANION 157600 then diff --git a/ROMFS/px4fmu_test/mixers/IO_pass.mix b/ROMFS/px4fmu_test/mixers/IO_pass.mix new file mode 100644 index 0000000000..42321f0ad9 --- /dev/null +++ b/ROMFS/px4fmu_test/mixers/IO_pass.mix @@ -0,0 +1,24 @@ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/makefiles/nuttx/upload.mk b/makefiles/nuttx/upload.mk index c590f17d10..e73c31dc31 100644 --- a/makefiles/nuttx/upload.mk +++ b/makefiles/nuttx/upload.mk @@ -15,7 +15,7 @@ ifeq ($(SYSTYPE),Darwin) SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index b5b0d5d9d9..5960414e36 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -179,6 +179,8 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */ + #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT #else @@ -1102,18 +1104,18 @@ L3GD20::test_error() int L3GD20::self_test() { - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* evaluate gyro offsets, complain if offset -> zero or larger than 25 dps */ + if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) return 1; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 90cc1d91b8..4034dbe3de 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -150,11 +150,12 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); /** * Engine Failure Current/Throttle Threshold * - * Engine failure triggers only below this current/throttle value + * Engine failure triggers only below this current value * * @group Commander * @min 0.0 - * @max 7.0 + * @max 30.0 + * @unit ampere */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -167,7 +168,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @group Commander * @unit second * @min 0.0 - * @max 7.0 + * @max 60.0 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index f45700eada..f496f9ed96 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -141,13 +141,13 @@ PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); /** * Velocity noise in down (vertical) direction * - * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 * - * @min 0.05 - * @max 5.0 + * @min 0.2 + * @max 3.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f); /** * Position noise in north-east (horizontal) direction @@ -163,21 +163,21 @@ PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); /** * Position noise in down (vertical) direction * - * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 * - * @min 0.1 - * @max 10.0 + * @min 0.5 + * @max 3.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f); /** * Magnetometer measurement noise * * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 * - * @min 0.1 - * @max 10.0 + * @min 0.01 + * @max 1.0 * @group Position Estimator */ PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); 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 33bc92967f..5f1c64b106 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 @@ -1762,11 +1762,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { + if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else { + } else if (_attitude_sp_pub <= 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 857c83f392..5570a184fb 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -33,9 +33,9 @@ /** * @file mavlink.c - * Adapter functions expected by the protocol library + * Define MAVLink specific parameters * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -46,7 +46,6 @@ #include "mavlink_bridge_header.h" #include -/* define MAVLink specific parameters */ /** * MAVLink system ID * @group MAVLink @@ -59,20 +58,36 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); * MAVLink component ID * @group MAVLink * @min 1 - * @max 50 + * @max 250 */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** - * MAVLink type + * MAVLink Radio ID + * + * When non-zero the MAVLink app will attempt to configure the + * radio to this ID and re-set the parameter to 0. If the value + * is negative it will reset the complete radio config to + * factory defaults. + * + * @group MAVLink + * @min -1 + * @max 240 + */ +PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); + +/** + * MAVLink airframe type + * + * @min 1 * @group MAVLink */ -PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); +PARAM_DEFINE_INT32(MAV_TYPE, 1); /** - * Use/Accept HIL GPS message (even if not in HIL mode) + * Use/Accept HIL GPS message even if not in HIL mode * - * If set to 1 incomming HIL GPS messages are parsed. + * If set to 1 incoming HIL GPS messages are parsed. * * @group MAVLink */ @@ -81,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); /** * Forward external setpoint messages * - * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard - * control mode + * If set to 1 incoming external setpoint messages will be directly forwarded + * to the controllers if in offboard control mode * * @group MAVLink */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c8427592d8..0edf7e4149 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -145,6 +145,7 @@ Mavlink::Mavlink() : _mavlink_ftp(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), + _radio_id(0), _logbuffer {}, _total_counter(0), _receive_thread {}, @@ -184,6 +185,7 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), + _param_radio_id(0), _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), _param_forward_externalsp(0), @@ -524,6 +526,7 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); @@ -539,6 +542,7 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates * after reboot - not during operation */ @@ -1662,6 +1666,50 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); } + /* radio config check */ + if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + usleep(1200000); + fprintf(fs, "+++\n"); + usleep(1200000); + + if (_radio_id > 0) { + /* set channel */ + fprintf(fs, "ATS3=%u\n", _radio_id); + usleep(200000); + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + usleep(200000); + + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. + //fclose(fs); + } else { + warnx("open fd %d failed", _uart_fd); + } + + /* reset param and save */ + _radio_id = 0; + param_set(_param_radio_id, &_radio_id); + } + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4a42f0bce1..d0ccd9622f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -355,6 +355,7 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; + int32_t _radio_id; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; @@ -424,6 +425,7 @@ private: bool _param_initialized; param_t _param_system_id; param_t _param_component_id; + param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; param_t _param_forward_externalsp; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 24f04fd74f..5bc7c0492e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1776,6 +1776,10 @@ protected: msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + msg.vx = pos_sp.vx; + msg.vy = pos_sp.vy; + msg.vz = pos_sp.vz; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } 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 2e9a92a4fd..6d866148e4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -122,6 +122,7 @@ private: int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ @@ -137,6 +138,7 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ @@ -319,6 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_alt_sp(true), _mode_auto(false) { + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); @@ -474,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions() { bool updated; + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + orb_check(_att_sub, &updated); if (updated) { @@ -903,6 +912,7 @@ MulticopterPositionControl::task_main() /* * do subscriptions */ + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1373,44 +1383,44 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - // generate attitude setpoint from manual controls + /* generate attitude setpoint from manual controls */ if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { - // reset yaw setpoint to current position if needed + /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _att.yaw; } - // do not move yaw while arming + /* do not move yaw while arming */ else if (_manual.z > 0.1f) { - const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); - if (yaw_offs < - YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); + float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(yaw_target - _att.yaw); - } else if (yaw_offs > YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); + // If the yaw offset became too big for the system to track stop + // shifting it + if (fabsf(yaw_offs) < yaw_offset_max) { + _att_sp.yaw_body = yaw_target; } } - //Control roll and pitch directly if we no aiding velocity controller is active + /* control roll and pitch directly if we no aiding velocity controller is active */ if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } - //Control climb rate directly if no aiding altitude controller is active + /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = math::min(_manual.z, _params.thr_max); _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); } - //Construct attitude setpoint rotation matrix + /* construct attitude setpoint rotation matrix */ math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); @@ -1435,10 +1445,9 @@ MulticopterPositionControl::task_main() if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr) { + if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { + } else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } 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 a09ed4a3e6..501cd695b8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2013-2015 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 @@ -36,7 +35,7 @@ * @file mc_pos_control_params.c * Multicopter position controller parameters. * - * @author Anton Babushkin + * @author Anton Babushkin */ #include @@ -107,7 +106,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 - * @max 8 m/s + * @max 8.0 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); @@ -184,7 +183,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); * * Limits maximum tilt in AUTO and POSCTRL modes during flight. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -196,7 +195,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * Limits maximum tilt angle on landing. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -215,7 +214,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); /** * Max manual roll * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -225,7 +224,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX, 35.0f); /** * Max manual pitch * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -235,7 +234,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); /** * Max manual yaw rate * - * @unit deg/s + * @unit degree / s * @min 0.0 * @group Multicopter Position Control */ diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index c14a4b7627..d5f8e668ce 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + } + if (!_control_mode->data().flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 5d7775bb23..a1ba7869ed 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 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 @@ -36,7 +36,7 @@ * * Parameters for DLL * - * @author Thomas Gubler + * @author Thomas Gubler */ #include @@ -64,7 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -75,7 +76,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); @@ -86,13 +88,14 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * Altitude of comms hold waypoint * * @unit m - * @min 0.0 + * @min -50 + * @max 30000 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * Aifield hole wait time + * Airfield hole wait time * * The amount of time in seconds the system should wait at the airfield home waypoint * diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b4f948fff0..e1d29334c6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 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 @@ -36,8 +36,8 @@ * * Parameters for navigator in general * - * @author Julian Oes - * @author Thomas Gubler + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -49,9 +49,9 @@ * * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * - * @unit meters - * @min 20 - * @max 200 + * @unit meter + * @min 25 + * @max 1000 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * * Default acceptance radius, overridden by acceptance radius of waypoint if set. * - * @unit meters + * @unit meter * @min 0.05 - * @max 200 + * @max 200.0 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); @@ -74,6 +74,7 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); @@ -84,6 +85,7 @@ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); @@ -94,7 +96,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * Latitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -105,7 +108,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * Longitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); @@ -116,7 +120,7 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * Altitude of airfield home waypoint * * @unit m - * @min 0.0 + * @min -50 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); 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 372b9b06da..6f60e4878d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -145,10 +145,9 @@ int position_estimator_inav_main(int argc, char *argv[]) inav_verbose_mode = false; - if (argc > 1) - if (!strcmp(argv[2], "-v")) { - inav_verbose_mode = true; - } + if (argc > 2 && !strcmp(argv[2], "-v")) { + inav_verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4123204601..ef097e648f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -680,6 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); @@ -690,6 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); @@ -700,6 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); @@ -736,6 +739,7 @@ PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); @@ -747,6 +751,7 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -758,6 +763,7 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); @@ -780,6 +786,7 @@ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); * * @min 0.0 * @max 100.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); @@ -787,10 +794,11 @@ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); /** * RC Channel 2 Minimum * - * Minimum value for RC channel 2 + * Minimum value for this channel. * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); @@ -798,27 +806,725 @@ PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); /** * RC Channel 2 Trim * - * Mid point value (same as min for throttle) + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); + +/** + * RC Channel 3 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); + +/** + * RC Channel 3 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); + +/** + * RC Channel 3 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); + +/** + * RC Channel 3 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); +/** + * RC Channel 3 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); + +/** + * RC Channel 4 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); + +/** + * RC Channel 4 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); + +/** + * RC Channel 4 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); + +/** + * RC Channel 4 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); + +/** + * RC Channel 4 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); + +/** + * RC Channel 5 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); + +/** + * RC Channel 5 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); + +/** + * RC Channel 5 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); + +/** + * RC Channel 5 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); + +/** + * RC Channel 5 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); + +/** + * RC Channel 6 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); + +/** + * RC Channel 6 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); + +/** + * RC Channel 6 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); + +/** + * RC Channel 6 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); + +/** + * RC Channel 6 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); + +/** + * RC Channel 7 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); + +/** + * RC Channel 7 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); + +/** + * RC Channel 7 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); + +/** + * RC Channel 7 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); + +/** + * RC Channel 7 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); + +/** + * RC Channel 8 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); + +/** + * RC Channel 8 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); + +/** + * RC Channel 8 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); + +/** + * RC Channel 8 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); + +/** + * RC Channel 8 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); + +/** + * RC Channel 9 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); + +/** + * RC Channel 9 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); + +/** + * RC Channel 9 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); + +/** + * RC Channel 9 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); + +/** + * RC Channel 9 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); + +/** + * RC Channel 10 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); + +/** + * RC Channel 10 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); + +/** + * RC Channel 10 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); + +/** + * RC Channel 10 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); + +/** + * RC Channel 10 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); + +/** + * RC Channel 11 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); + +/** + * RC Channel 11 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); + +/** + * RC Channel 11 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); + +/** + * RC Channel 11 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); + +/** + * RC Channel 11 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); + +/** + * RC Channel 12 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); + +/** + * RC Channel 12 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); + +/** + * RC Channel 12 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); + +/** + * RC Channel 12 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); + +/** + * RC Channel 12 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); + +/** + * RC Channel 13 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); + +/** + * RC Channel 13 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); + +/** + * RC Channel 13 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); + +/** + * RC Channel 13 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); + +/** + * RC Channel 13 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); + +/** + * RC Channel 14 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); + +/** + * RC Channel 14 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); /** - * RC Channel 2 Maximum + * RC Channel 14 Maximum * - * Maximum value for RC channel 2 + * Maximum value for this channel. * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); /** - * RC Channel 2 Reverse + * RC Channel 14 Reverse * * Set to -1 to reverse channel. * @@ -826,10 +1532,10 @@ PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); * @max 1.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); /** - * RC Channel 2 dead zone + * RC Channel 14 dead zone * * The +- range of this value around the trim value will be considered as zero. * @@ -837,105 +1543,248 @@ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); * @max 100.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC3_MIN, 1000); -PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC3_MAX, 2000); -PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC4_MIN, 1000); -PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC4_MAX, 2000); -PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC5_MIN, 1000); -PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC5_MAX, 2000); -PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC6_MIN, 1000); -PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC6_MAX, 2000); -PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC7_MIN, 1000); -PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC7_MAX, 2000); -PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC8_MIN, 1000); -PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC8_MAX, 2000); -PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC9_MIN, 1000); -PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC9_MAX, 2000); -PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC10_MIN, 1000); -PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC10_MAX, 2000); -PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC11_MIN, 1000); -PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC11_MAX, 2000); -PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC12_MIN, 1000); -PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC12_MAX, 2000); -PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC13_MIN, 1000); -PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC13_MAX, 2000); -PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC14_MIN, 1000); -PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC14_MAX, 2000); -PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +/** + * RC Channel 15 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_MIN, 1000); + +/** + * RC Channel 15 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); + +/** + * RC Channel 15 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); + +/** + * RC Channel 15 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); + +/** + * RC Channel 15 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); +/** + * RC Channel 16 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_MIN, 1000); + +/** + * RC Channel 16 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); + +/** + * RC Channel 16 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_MAX, 2000); + +/** + * RC Channel 16 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); + +/** + * RC Channel 16 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); +/** + * RC Channel 17 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_MIN, 1000); + +/** + * RC Channel 17 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); + +/** + * RC Channel 17 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_MAX, 2000); + +/** + * RC Channel 17 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); + +/** + * RC Channel 17 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); +/** + * RC Channel 18 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_MIN, 1000); + +/** + * RC Channel 18 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); + +/** + * RC Channel 18 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_MAX, 2000); + +/** + * RC Channel 18 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); + +/** + * RC Channel 18 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +/** + * Enable relay control of relay 1 mapped to the Spektrum receiver power supply + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif @@ -952,6 +1801,8 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /** * Scaling factor for battery voltage sensor on PX4IO. * + * @min 1 + * @max 100000 * @group Battery Calibration */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); @@ -1231,8 +2082,12 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. * - * @min 800 + * Set to a value slightly above the PWM value assumed by throttle in a failsafe event, + * but ensure it is below the PWM value assumed by throttle during normal operation. + * + * @min 0 * @max 2200 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index f302314a23..33c095036c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -43,10 +43,10 @@ /** * VTOL number of engines * - * @min 1 + * @min 0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT, 0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); /** * Minimum airspeed in multicopter mode @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f); /** * Trim airspeed when in multicopter mode @@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f); /** * Permanent stabilization in fw mode @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); +PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); /** * Fixed wing pitch trim @@ -107,7 +107,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f); /** * Motor max power @@ -118,18 +118,18 @@ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); * @min 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); +PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); /** * Propeller efficiency parameter * * Influences propeller efficiency at different power settings. Should be tuned beforehand. * - * @min 0.5 + * @min 0.0 * @max 0.9 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); +PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); /** * Total airspeed estimate low-pass filter gain @@ -140,7 +140,7 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); * @max 0.99 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** * VTOL Type (Tailsitter=0, Tiltrotor=1) @@ -160,4 +160,4 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 44ae4df068..bda49ae86d 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -52,8 +52,8 @@ void _assert_parameter_int_value(param_t param, int32_t expected) { int32_t value; int result = param_get(param, &value); - ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); - ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); + ASSERT_EQ(0, result) << printf("param_get (%lu) did not return parameter\n", param); + ASSERT_EQ(expected, value) << printf("value for param (%lu) doesn't match default value\n", param); } void _set_all_int_parameters_to(int32_t value)