From 66cdfeca6235f6ee7254e0d194e443ed257150d9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 21 Jun 2015 00:31:23 +0300 Subject: [PATCH 01/20] ROMFS: Enable FTP on companion link Not sure that ftp usable in 57600. Tested on 921600. --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8a93737ef8..0fb6e117cc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x fi if param compare SYS_COMPANION 157600 then From 8fa161b7c4947ccc988a737d2fd86610bca8cab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:58:06 +0200 Subject: [PATCH 02/20] Multicopter configs: Remove duplicate defaults, each line checked to match new param-level defaults --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 32 ----------------------- 1 file changed, 32 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 1f34282aec..fa3653e0d5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,38 +4,6 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - param set PE_VELNE_NOISE 0.5 param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 From 9365c5a4383d8702e66b5f0e3f95a1acb3fd5cac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:59:28 +0200 Subject: [PATCH 03/20] systemlib: Remove file present 2x from Makefile --- ROMFS/px4fmu_common/init.d/rc.usb | 6 +++--- src/modules/systemlib/module.mk | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0442637941..027d2aca5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,21 +3,21 @@ # USB MAVLink start # -mavlink start -r 80000 -d /dev/ttyACM0 -x +mavlink start -r 800000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20 +mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 # Exit shell to make it available to MAVLink diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f80d8009ae..f2499bbb13 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,8 +55,7 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c \ - circuit_breaker_params.c + mcu_version.c MAXOPTIMIZATION = -Os From 62b102d0b49f5cf47f5623761aed4db2a390ceaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:06 +0200 Subject: [PATCH 04/20] MC attitude controller: Set better defaults --- .../mc_att_control/mc_att_control_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 c9bf3753b4..c0f110123a 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -50,7 +50,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); /** * Roll rate P gain @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f); /** * Roll rate D gain @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); /** * Roll rate feedforward @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); /** * Pitch rate P gain @@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f); /** * Pitch rate D gain @@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); /** * Pitch rate feedforward @@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); /** * Yaw rate P gain @@ -162,7 +162,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); /** * Yaw rate I gain @@ -172,7 +172,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); /** * Yaw rate D gain From 2c2a6b710c13f835d408a66104a47e73fb3685ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:23 +0200 Subject: [PATCH 05/20] MC position controller: Set better defaults --- src/modules/mc_pos_control/mc_pos_control_params.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 4865c1c684..a09ed4a3e6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -107,9 +107,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 + * @max 8 m/s * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); /** * Vertical velocity feed forward From 1c82f73822c1523be1bcaefd3b7986ddb8c59bdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:15:45 +0200 Subject: [PATCH 06/20] Dataman: Reduce excessive stack allocation --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b442b74303..6cfbb4d830 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -794,7 +794,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1500, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From d673bf8457cdfe7fede5097e3b932645d07f1c62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:16:03 +0200 Subject: [PATCH 07/20] Navigator: Reduce excessive stack allocation --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1460972cc2..dc97600434 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 1700, + 1500, (main_t)&Navigator::task_main_trampoline, nullptr); From ff3977366607d1c54beb605029764b88261207b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:22 +0200 Subject: [PATCH 08/20] MC: Better attitude control defaults --- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c0f110123a..42c7bc3d04 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f); /** * Roll rate I gain @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f); /** * Pitch rate I gain From ae9f1ec955906038652c459e6343b0d30f816722 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:34 +0200 Subject: [PATCH 09/20] CAN config: Better attitude control defaults --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 05b4355138..c6341a4f74 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -9,18 +9,17 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 4c975a11e5aa0402d3179e772d6580b71c4ae2de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:33:07 +0200 Subject: [PATCH 10/20] param command: Complete help text --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 45fb2fd830..210d93bde8 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -189,7 +189,7 @@ param_main(int argc, char *argv[]) } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'"); } static void From c192398a6517648d3a46729c2ddf7268beed89a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:19 +0200 Subject: [PATCH 11/20] mavlink app: Be more verbose on param load fails --- src/modules/mavlink/mavlink_parameters.cpp | 23 +++++++++++++++++----- src/modules/mavlink/mavlink_parameters.h | 5 +++-- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 524effb205..73d7580d21 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 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,6 +36,7 @@ * Mavlink parameters manager implementation. * * @author Anton Babushkin + * @author Lorenz Meier */ #include @@ -130,7 +131,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_used_index(req_read.param_index)); + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } } } break; @@ -207,11 +218,11 @@ MavlinkParametersManager::send(const hrt_abstime t) } } -void +int MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { - return; + return 1; } mavlink_param_value_t msg; @@ -221,7 +232,7 @@ MavlinkParametersManager::send_param(param_t param) * space during transmission, copy param onto float val_buf */ if (param_get(param, &msg.param_value) != OK) { - return; + return 2; } msg.param_count = param_count_used(); @@ -248,4 +259,6 @@ MavlinkParametersManager::send_param(param_t param) } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b6736f2128..3dfed084b3 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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,6 +36,7 @@ * Mavlink parameters manager definition. * * @author Anton Babushkin + * @author Lorenz Meier */ #pragma once @@ -113,7 +114,7 @@ protected: void send(const hrt_abstime t); - void send_param(param_t param); + int send_param(param_t param); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; From f1582e67de83a4c5b30e22f5322b5686613c0580 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:42 +0200 Subject: [PATCH 12/20] F330 config: Better default gains --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index d07e926a79..512ad132be 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -10,11 +10,11 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 From 7b588c5bd0a0b3c0cba09a16a3657ba44022b3fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:07:49 +0200 Subject: [PATCH 13/20] Fix F450 default gains --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 9b3954be6f..2a77f13866 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -11,15 +11,15 @@ if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 26c47f25cb9613f1ceb9fffe04eef3d0cc70c293 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:37:56 +0200 Subject: [PATCH 14/20] PWM outputs: Allow the new p:PWM_OUT etc params for setting PWM limits via params at boot-time. --- src/modules/sensors/sensor_params.c | 83 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 6 +++ src/systemcmds/pwm/pwm.c | 32 +++++++++-- 3 files changed, 118 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7b3932638b..83568c0069 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1401,3 +1401,86 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); * */ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); + +/** + * Enable Lidar-Lite (LL40LS) pwm driver + * + * @min 0 + * @max 1 + * @group Sensor Enable + */ +PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAX, 2000); + +/** + * Set the disarmed PWM for MAIN outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_DISARMED, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); + +/** + * Set the disarmed PWM for AUX outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1000); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1e831becd9..0c7b0467f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -634,6 +634,12 @@ Sensors::Sensors() : (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); + (void)param_find("PWM_MIN"); + (void)param_find("PWM_MAX"); + (void)param_find("PWM_DISARMED"); + (void)param_find("PWM_AUX_MIN"); + (void)param_find("PWM_AUX_MAX"); + (void)param_find("PWM_AUX_DISARMED"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 6bb9f235cb..168a1d8603 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -59,6 +59,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" #include "drivers/drv_pwm_output.h" static void usage(const char *reason); @@ -187,10 +188,35 @@ pwm_main(int argc, char *argv[]) break; case 'p': - pwm_value = strtoul(optarg, &ep, 0); + { + /* check if this is a param name */ + if (strncmp("p:", optarg, 2) == 0) { - if (*ep != '\0') { - usage("BAD PWM VAL"); + char buf[32]; + strncpy(buf, optarg + 2, 16); + /* user wants to use a param name */ + param_t parm = param_find(buf); + + if (parm != PARAM_INVALID) { + int32_t pwm_parm; + int gret = param_get(parm, &pwm_parm); + + if (gret == 0) { + pwm_value = pwm_parm; + } else { + usage("PARAM LOAD FAIL"); + } + } else { + usage("PARAM NAME NOT FOUND"); + } + } else { + + pwm_value = strtoul(optarg, &ep, 0); + } + + if (*ep != '\0') { + usage("BAD PWM VAL"); + } } break; From 20d735701f6c4806d246e4d8fd75758f698b40bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:40:12 +0200 Subject: [PATCH 15/20] sensor params: Add hint to reboot system after changing PWM params --- src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 83568c0069..72d139b113 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1414,6 +1414,10 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1426,6 +1430,10 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1438,6 +1446,10 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000); /** * Set the disarmed PWM for MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * @@ -1451,6 +1463,10 @@ PARAM_DEFINE_INT32(PWM_DISARMED, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1463,6 +1479,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1475,6 +1495,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); /** * Set the disarmed PWM for AUX outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * From 2284aa0c96d5abbe132931c7bcb91710c505fd24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 00:47:20 +0200 Subject: [PATCH 16/20] Caipi config: Move to param based config --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 1fd96d6d3d..8a661f25e2 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -34,7 +34,9 @@ then param set PWM_MAIN_REV1 1 fi +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + set MIXER caipi -# Provide ESC a constant 1000 us pulse -set PWM_OUT 4 -set PWM_DISARMED 1000 +set PWM_OUT 1234 From 52b0f17ff31213e1c073cf53c069e8883a3ca0e9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:29:25 +0200 Subject: [PATCH 17/20] increase highest pwm to 2150 --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 2fb9469c8a..18a75d063f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -83,7 +83,7 @@ __BEGIN_DECLS /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2100 +#define PWM_HIGHEST_MAX 2150 /** * Default maximum PWM in us From 0f21733cfc38edda892237bbb57deefd0de8b713 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:42:48 +0200 Subject: [PATCH 18/20] Wing-wing: Remove unused params. Camflyer: Copy wing-wing defaults --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 24 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3033_wingwing | 10 --------- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 1886783249..040b78dc77 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,6 +2,30 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.005 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.04 +fi + set MIXER Q # Provide ESC a constant 1000 us pulse while disarmed set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index add905b115..708c34491b 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -30,16 +30,6 @@ then param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set MT_TKF_PIT_MAX 30.0 - param set MT_ACC_D 0.2 - param set MT_ACC_P 0.6 - param set MT_A_LP 0.5 - param set MT_PIT_OFF 0.1 - param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.65 - param set MT_THR_I 0.35 - param set MT_THR_P 0.2 - param set MT_THR_FF 1.5 fi set MIXER wingwing From 289ad91bcc2e49c33ae338152d7af19a13c4b64d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:11 +0200 Subject: [PATCH 19/20] Fixed wing land detector: Filter GPS speeds more since they are unreliable, leave airspeed filter where it was --- src/modules/land_detector/FixedwingLandDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5f7ded9cb2..741bc02ad4 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -85,12 +85,12 @@ bool FixedwingLandDetector::update() bool landDetected = false; if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); if (isfinite(val)) { _velocity_xy_filtered = val; } - val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (isfinite(val)) { _velocity_z_filtered = val; From 640024357f3b3a261031b750cf7a7b5a82e53a78 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:44 +0200 Subject: [PATCH 20/20] Land detector: increase ground speed threshold --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index b670dcc035..f182495ac3 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate