From 3010f313dc2e291d3b2f1f6022f132ce227b1895 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Mar 2015 09:04:01 +0100 Subject: [PATCH 01/32] Fix IO update when safety can not be set to on. From @zottgrammes Conflicts: ROMFS/px4fmu_common/init.d/rcS --- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 580043a1d4..816b1152fe 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -198,8 +198,17 @@ then tone_alarm MLL32CP8MB - px4io start - px4io safety_on + if px4io start + then + # try to safe px4 io so motor outputs dont go crazy + if px4io safety_on + then + # success! no-op + else + # px4io did not respond to the safety command + px4io stop + fi + fi if px4io forceupdate 14662 ${IO_FILE} then From dac74db104f8fda5bd3b8b68f3b7f45878a27865 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 17:53:19 +0200 Subject: [PATCH 02/32] change the nested if structure which tries all i2c busses to a loop. --- src/drivers/px4flow/px4flow.cpp | 81 ++++++++++++++++++--------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 2166258c15..8bf20290ca 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -616,65 +616,74 @@ start() errx(1, "already started"); } - /* create the driver */ - g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, #ifdef PX4_I2C_BUS_ESC - delete g_dev; - /* try 2nd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; + const int *cur_bus = busses_to_try; + while(*cur_bus != -1) { + /* create the driver */ + //warnx("trying bus %d", *cur_bus); + g_dev = new PX4FLOW(*cur_bus); if (g_dev == nullptr) { - goto fail; + /* this is a fatal error */ + break; } - - if (OK != g_dev->init()) { - #endif - - delete g_dev; - /* try 3rd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - #ifdef PX4_I2C_BUS_ESC + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; } - #endif + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - + + /* check whether we found it: */ + if (*cur_bus == -1) { + goto not_found; + } + + /* check for failure: */ + if (g_dev == nullptr) { + goto fatal_fail; + } + /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - goto fail; + goto fatal_fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fail; + goto fatal_fail; } exit(0); -fail: +not_found: + /* for now we do the same as if there was a fatal failure. */ + warnx("PX4FLOW not found on I2C busses"); + +fatal_fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - errx(1, "no PX4FLOW connected over I2C"); + errx(1, "PX4FLOW could not be started over I2C"); } /** From d9e6cb0f584f6e4016688a12a03e641af47cff4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 18:28:19 +0200 Subject: [PATCH 03/32] implemented retrying the connection to the px4flow sensor before giving up. --- src/drivers/px4flow/px4flow.cpp | 133 ++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8bf20290ca..8ee365d8df 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -596,7 +596,11 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev = nullptr; +bool start_in_progress = false; + +const int START_RETRY_COUNT = 5; +const int START_RETRY_TIMEOUT = 1000; void start(); void stop(); @@ -611,78 +615,93 @@ void start() { int fd; + + /* entry check: */ + if (start_in_progress) { + errx(1, "start in progress"); + } + start_in_progress = true; if (g_dev != nullptr) { + start_in_progress = false; errx(1, "already started"); } - const int busses_to_try[] = { - PX4_I2C_BUS_EXPANSION, - #ifdef PX4_I2C_BUS_ESC - PX4_I2C_BUS_ESC, - #endif - PX4_I2C_BUS_ONBOARD, - -1 - }; + int retry_nr = 0; + while (1) { + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, + #ifdef PX4_I2C_BUS_ESC + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; - const int *cur_bus = busses_to_try; - while(*cur_bus != -1) { - /* create the driver */ - //warnx("trying bus %d", *cur_bus); - g_dev = new PX4FLOW(*cur_bus); - if (g_dev == nullptr) { - /* this is a fatal error */ - break; + const int *cur_bus = busses_to_try; + + while(*cur_bus != -1) { + /* create the driver */ + /* warnx("trying bus %d", *cur_bus); */ + g_dev = new PX4FLOW(*cur_bus); + if (g_dev == nullptr) { + /* this is a fatal error */ + break; + } + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; + } + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - - /* init the driver: */ - if (OK == g_dev->init()) { + + /* check whether we found it: */ + if (*cur_bus != -1) { + + /* check for failure: */ + if (g_dev == nullptr) { + break; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + break; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + break; + } + /* success! */ + start_in_progress = false; + exit(0); + } + + if (retry_nr < START_RETRY_COUNT) { + warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); + usleep(START_RETRY_TIMEOUT * 1000); + retry_nr++; + } else { break; } - - /* destroy it again because it failed. */ - delete g_dev; - g_dev = nullptr; - - /* try next! */ - cur_bus++; } - /* check whether we found it: */ - if (*cur_bus == -1) { - goto not_found; - } - - /* check for failure: */ - if (g_dev == nullptr) { - goto fatal_fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fatal_fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fatal_fail; - } - - exit(0); - -not_found: - /* for now we do the same as if there was a fatal failure. */ - warnx("PX4FLOW not found on I2C busses"); - -fatal_fail: - if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - + + start_in_progress = false; errx(1, "PX4FLOW could not be started over I2C"); } From 440aedebadc3db8ee750736ab297b25a4c04ec4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 21:10:48 +0200 Subject: [PATCH 04/32] change start script to launch the px4flow driver in background. Fixes issue #2145 --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 474db36ef7..e5d527dd71 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -111,7 +111,7 @@ else fi # Check for flow sensor -if px4flow start +if px4flow start & then fi From cf8307f039f328a42afc719fe1a74492c7edb527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 18:57:59 +0200 Subject: [PATCH 05/32] Commander: Low-pass battery throttle to better match battery dynamics --- src/modules/commander/commander_helper.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 68949eec1d..491ce32a5c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -108,6 +108,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -381,8 +382,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ From 6e0aa90bb832d4cf9a0d5e3619914f91109cbe7b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 18:57:59 +0200 Subject: [PATCH 06/32] Commander: Low-pass battery throttle to better match battery dynamics --- src/modules/commander/commander_helper.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 68949eec1d..491ce32a5c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -108,6 +108,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -381,8 +382,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ From a74cc5bf494b05f48e796190d3ea3e43c849aa31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 19:07:08 +0200 Subject: [PATCH 07/32] MAVLink app: Fix scaling of battery current --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0b6dfcab5f..cb56331e95 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -537,7 +537,7 @@ protected: msg.onboard_control_sensors_health = status.onboard_control_sensors_health; msg.load = status.load * 1000.0f; msg.voltage_battery = status.battery_voltage * 1000.0f; - msg.current_battery = status.battery_current * 100.0f; + msg.current_battery = status.battery_current / 10.0f; msg.drop_rate_comm = status.drop_rate_comm; msg.errors_comm = status.errors_comm; msg.errors_count1 = status.errors_count1; @@ -562,7 +562,7 @@ protected: bat_msg.voltages[i] = 0; } } - bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_battery = status.battery_current / 10.0f; bat_msg.current_consumed = status.battery_discharged_mah; bat_msg.energy_consumed = -1.0f; bat_msg.battery_remaining = (status.battery_voltage > 0) ? From 2adb48ce9004bff96fc6e2508f30ecdb49b398a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:48:07 +0200 Subject: [PATCH 08/32] MC pos control: Better default velocity gain. --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 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 501cd695b8..a3793b0cc0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); /** * Integral gain for vertical velocity error From fd63ba7b898b95469b62f6f3f42f12e042aae619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jul 2015 01:43:00 +0200 Subject: [PATCH 09/32] FW pos control: Widen acceptance range for yaw rate to re-engage heading hold --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b5436602ec..8856dd829d 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 @@ -97,7 +97,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane -#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode +#define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) From 5f586fc354caf94facaf79ca5702ddc57ef4a982 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:10 +0200 Subject: [PATCH 10/32] Mixer library: Fix code style --- src/modules/px4iofirmware/mixer.cpp | 35 ++++-- src/modules/systemlib/mixer/mixer.cpp | 24 +++-- src/modules/systemlib/mixer/mixer.h | 34 +++--- src/modules/systemlib/mixer/mixer_load.c | 16 ++- .../systemlib/mixer/mixer_multirotor.cpp | 102 ++++++++++-------- src/modules/systemlib/mixer/mixer_simple.cpp | 42 +++++--- 6 files changed, 157 insertions(+), 96 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0106fa1eb7..a2cfdf491b 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -35,6 +35,8 @@ * @file mixer.cpp * * Control channel input/output mixer and failsafe. + * + * @author Lorenz Meier */ #include @@ -64,6 +66,7 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; +static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; @@ -172,6 +175,11 @@ mixer_tick(void) ) ); + should_arm_nothrottle = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); @@ -237,24 +245,25 @@ mixer_tick(void) /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + /* clamp unused outputs to zero */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; + outputs[i] = 0; + } + /* store normalized outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } /* set arming */ - bool needs_to_arm = (should_arm || should_always_enable_pwm); + bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); /* check any conditions that prevent arming */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { needs_to_arm = false; } - if (!should_arm && !should_always_enable_pwm) { - needs_to_arm = false; - } if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -308,8 +317,9 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group >= PX4IO_CONTROL_GROUPS) + if (control_group >= PX4IO_CONTROL_GROUPS) { return -1; + } switch (source) { case MIX_FMU: @@ -359,8 +369,8 @@ mixer_callback(uintptr_t handle, } } - /* motor spinup phase - lock throttle to zero */ - if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + /* motor spinup phase or only safety off, but not armed - lock throttle to zero */ + if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, @@ -458,8 +468,9 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) + if (resid > 0) { memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + } mixer_text_length = resid; @@ -482,8 +493,9 @@ mixer_set_failsafe() */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; + } /* set failsafe defaults to the values for all inputs = 0 */ float outputs[PX4IO_SERVO_COUNT]; @@ -501,7 +513,8 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servo_failsafe[i] = 0; + } } diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 3ab41c5c58..cd2010b92c 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input) int Mixer::scale_check(struct mixer_scaler_s &scaler) { - if (scaler.offset > 1.001f) + if (scaler.offset > 1.001f) { return 1; + } - if (scaler.offset < -1.001f) + if (scaler.offset < -1.001f) { return 2; + } - if (scaler.min_output > scaler.max_output) + if (scaler.min_output > scaler.max_output) { return 3; + } - if (scaler.min_output < -1.001f) + if (scaler.min_output < -1.001f) { return 4; + } - if (scaler.max_output > 1.001f) + if (scaler.max_output > 1.001f) { return 5; + } return 0; } @@ -120,11 +125,14 @@ const char * Mixer::findtag(const char *buf, unsigned &buflen, char tag) { while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) + if ((buf[0] == tag) && (buf[1] == ':')) { return buf; + } + buf++; buflen--; } + return nullptr; } @@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen) /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { return nm; } diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1190683015..cd98141905 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -222,7 +222,7 @@ protected: * @param buflen length of the buffer. * @param tag character to search for. */ - static const char * findtag(const char *buf, unsigned &buflen, char tag); + static const char *findtag(const char *buf, unsigned &buflen, char tag); /** * Skip a line @@ -231,13 +231,13 @@ protected: * @param buflen length of the buffer. * @return 0 / OK if a line could be skipped, 1 else */ - static const char * skipline(const char *buf, unsigned &buflen); + static const char *skipline(const char *buf, unsigned &buflen); private: /* do not allow to copy due to prt data members */ - Mixer(const Mixer&); - Mixer& operator=(const Mixer&); + Mixer(const Mixer &); + Mixer &operator=(const Mixer &); }; /** @@ -316,8 +316,8 @@ private: Mixer *_first; /**< linked list of mixers */ /* do not allow to copy due to pointer data members */ - MixerGroup(const MixerGroup&); - MixerGroup operator=(const MixerGroup&); + MixerGroup(const MixerGroup &); + MixerGroup operator=(const MixerGroup &); }; /** @@ -437,8 +437,8 @@ private: uint8_t &control_index); /* do not allow to copy due to ptr data members */ - SimpleMixer(const SimpleMixer&); - SimpleMixer operator=(const SimpleMixer&); + SimpleMixer(const SimpleMixer &); + SimpleMixer operator=(const SimpleMixer &); }; /** @@ -449,13 +449,13 @@ private: typedef unsigned int MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; -/** - * Multi-rotor mixer for pre-defined vehicle geometries. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to - * a set of outputs based on the configured geometry. - */ -class __EXPORT MultirotorMixer : public Mixer + /** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ + class __EXPORT MultirotorMixer : public Mixer { public: /** @@ -531,8 +531,8 @@ private: const Rotor *_rotors; /* do not allow to copy due to ptr data members */ - MultirotorMixer(const MultirotorMixer&); - MultirotorMixer operator=(const MultirotorMixer&); + MultirotorMixer(const MultirotorMixer &); + MultirotorMixer operator=(const MultirotorMixer &); }; #endif diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 0d629d6100..c0950d77a7 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); + if (fp == NULL) { warnx("file not found"); return -1; @@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* read valid lines from the file into a buffer */ buf[0] = '\0'; + for (;;) { /* get a line, bail on error/EOF */ line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) + + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { continue; + } /* compact whitespace in the buffer */ char *t, *f; + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ t = f + 1; - while (*t == ' ') + + while (*t == ' ') { t++; + } + if (*t == '\0') { /* strip trailing whitespace */ *f = '\0'; + } else if (t > (f + 1)) { memmove(f + 1, t, strlen(t) + 1); } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index aa0a8e7418..6bbc349d87 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -109,15 +109,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]); return nullptr; } @@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return nullptr; @@ -170,7 +173,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorGeometry::OCTA_X; - + } else if (!strcmp(geomname, "8c")) { geometry = MultirotorGeometry::OCTA_COX; @@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (status_reg != NULL) { (*status_reg) = 0; } + // thrust boost parameters float thrust_increase_factor = 1.5f; float thrust_decrease_factor = 0.6f; @@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (out < min_out) { min_out = out; } + if (out > max_out) { max_out = out; } @@ -248,87 +253,94 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - if(max_thrust_diff >= -min_out) { + + if (max_thrust_diff >= -min_out) { boost = -min_out; - } - else { + + } else { boost = max_thrust_diff; - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } - } - else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - if(max_thrust_diff >= max_out - 1.0f) { + + } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + + if (max_thrust_diff >= max_out - 1.0f) { boost = -(max_out - 1.0f); + } else { boost = -max_thrust_diff; - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } - else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + + } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); - } - else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); - } - else if (min_out < 0.0f && max_out > 1.0f) { - boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); + + } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + + } else if (min_out < 0.0f && max_out > 1.0f) { + boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, + thrust_increase_factor * thrust - thrust); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } // notify if saturation has occurred - if(min_out < 0.0f) { - if(status_reg != NULL) { + if (min_out < 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } } - if(max_out > 0.0f) { - if(status_reg != NULL) { + + if (max_out > 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } } // mix again but now with thrust boost, scale roll/pitch and also add yaw - for(unsigned i = 0; i < _rotor_count; i++) { + for (unsigned i = 0; i < _rotor_count; i++) { float out = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + thrust + boost; out *= _rotors[i].out_scale; // scale yaw if it violates limits. inform about yaw limit reached - if(out < 0.0f) { + if (out < 0.0f) { yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; - if(status_reg != NULL) { + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } - } - else if(out > 1.0f) { + + } else if (out > 1.0f) { // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; - if(status_reg != NULL) { + roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } } } - /* last mix, add yaw and scale outputs to range idle_speed...1 */ + /* add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + - thrust + boost; + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index e48bda6918..5c2edef61b 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb, SimpleMixer::~SimpleMixer() { - if (_info != nullptr) + if (_info != nullptr) { free(_info); + } } int @@ -77,8 +78,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler int ret; int s[5]; int n = -1; - + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) { debug("output parser failed finding tag, ret: '%s'", buf); return -1; @@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } int -SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index) { unsigned u[2]; int s[5]; buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) { debug("control parser failed finding tag, ret: '%s'", buf); return -1; @@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); goto out; @@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } SimpleMixer * -SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, + uint16_t mid, uint16_t max) { SimpleMixer *sm = nullptr; mixer_simple_s *mixinfo = nullptr; @@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } @@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; - if (_info == nullptr) + if (_info == nullptr) { return 0; + } - if (space < 1) + if (space < 1) { return 0; + } for (unsigned i = 0; i < _info->control_count; i++) { float input; @@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) void SimpleMixer::groups_required(uint32_t &groups) { - for (unsigned i = 0; i < _info->control_count; i++) + for (unsigned i = 0; i < _info->control_count; i++) { groups |= 1 << _info->controls[i].control_group; + } } int @@ -305,14 +318,16 @@ SimpleMixer::check() /* sanity that presumes that a mixer includes a control no more than once */ /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) + if (_info->control_count > 32) { return -2; + } /* validate the output scaler */ ret = scale_check(_info->output_scaler); - if (ret != 0) + if (ret != 0) { return ret; + } /* validate input scalers */ for (unsigned i = 0; i < _info->control_count; i++) { @@ -328,8 +343,9 @@ SimpleMixer::check() /* validate the scaler */ ret = scale_check(_info->controls[i].scaler); - if (ret != 0) + if (ret != 0) { return (10 * i + ret); + } } return 0; From 6fe717b17a75412c3e5e18c3e678c9cc3285cab5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jul 2015 12:05:45 +0200 Subject: [PATCH 11/32] Default MAVLink component ID to 1, since that is the more common assumption in the field --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index b1d369cbf4..df2a5786ad 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -60,7 +60,7 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); * @min 1 * @max 250 */ -PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +PARAM_DEFINE_INT32(MAV_COMP_ID, 1); /** * MAVLink Radio ID From 8bb9707f3fc5a87213bb567a61416d9d90e6b239 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:38 +0200 Subject: [PATCH 12/32] FMU: Allow to pre-arm the non-throttle channels with the safety switch --- src/drivers/px4fmu/fmu.cpp | 60 +++++++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2047046b9b..b1766f7390 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -90,6 +90,7 @@ */ #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev { @@ -136,7 +137,6 @@ private: int _armed_sub; int _param_sub; orb_advert_t _outputs_pub; - actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; @@ -156,6 +156,7 @@ private: unsigned _poll_fds_num; static pwm_limit_t _pwm_limit; + static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -164,6 +165,8 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; + static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -240,8 +243,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { #endif }; -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); -pwm_limit_t PX4FMU::_pwm_limit; +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; +actuator_armed_s PX4FMU::_armed = {}; namespace { @@ -261,7 +265,6 @@ PX4FMU::PX4FMU() : _armed_sub(-1), _param_sub(-1), _outputs_pub(-1), - _armed{}, _num_outputs(0), _class_instance(0), _task_should_exit(false), @@ -695,24 +698,17 @@ PX4FMU::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + /* disable unused ports by setting their output to NaN */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN and INF */ - if ((i >= outputs.noutputs) || - !isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; + if (i >= outputs.noutputs) { + outputs.output[i] = NAN_VALUE; } } uint16_t pwm_limited[num_outputs]; - /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { @@ -737,13 +733,14 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; - if (_servo_armed != set_armed) + if (_servo_armed != set_armed) { _servo_armed = set_armed; + } /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = (set_armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -828,6 +825,13 @@ PX4FMU::control_callback(uintptr_t handle, input = controls[control_group].control[control_index]; + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + } else if (input < -1.0f) { + input = -1.0f; + } + /* motor spinup phase - lock throttle to zero */ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && @@ -839,6 +843,15 @@ PX4FMU::control_callback(uintptr_t handle, } } + /* throttle not arming - mark throttle input as invalid */ + if (arm_nothrottle()) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + return 0; } @@ -847,14 +860,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); - if (ret != -ENOTTY) + if (ret != -ENOTTY) { return ret; + } /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { @@ -873,8 +884,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } From 0ca6f46ef497419d50cae2cc14be061aafe8ed73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:59 +0200 Subject: [PATCH 13/32] IO: Allow to pre-arm the non-throttle channels with the safety switch --- src/modules/px4iofirmware/mixer.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2cfdf491b..050750d080 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,6 +62,7 @@ extern "C" { * Maximum interval in us before FMU signal is considered lost */ #define FMU_INPUT_DROP_LIMIT_US 500000 +#define NAN_VALUE (0.0f/0.0f) /* current servo arm/disarm state */ static bool mixer_servos_armed = false; @@ -243,12 +244,12 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; - outputs[i] = 0; + outputs[i] = 0.0f; } /* store normalized outputs */ @@ -369,7 +370,14 @@ mixer_callback(uintptr_t handle, } } - /* motor spinup phase or only safety off, but not armed - lock throttle to zero */ + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_THROTTLE) { @@ -380,11 +388,13 @@ mixer_callback(uintptr_t handle, } } - /* limit output */ - if (control > 1.0f) { - control = 1.0f; - } else if (control < -1.0f) { - control = -1.0f; + /* only safety off, but not armed - set throttle as invalid */ + if (should_arm_nothrottle && !should_arm) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* mark the throttle as invalid */ + control = NAN_VALUE; + } } return 0; From 433c9bf42d55165c9e45616514cda2765217af63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:52:15 +0200 Subject: [PATCH 14/32] PWM limit lib: Support pre-arming --- src/modules/systemlib/pwm_limit/pwm_limit.c | 28 ++++++++++++++++++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 2f72d347c6..09965b96b9 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit) return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, +void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { @@ -99,6 +99,16 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ break; } + /* if the system is pre-armed, the limit state is temporarily on, + * as some outputs are valid and the non-valid outputs have been + * set to NaN. This is not stored in the state machine though, + * as the throttle channels need to go through the ramp at + * regular arming time. + */ + if (pre_armed) { + limit->state = PWM_LIMIT_STATE_ON; + } + unsigned progress; /* then set effective_pwm based on state */ @@ -120,6 +130,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:35:11 +0200 Subject: [PATCH 15/32] Mixer test: Add routine to test pre-arming --- src/systemcmds/tests/test_mixer.cpp | 64 +++++++++++++++++++++++------ 1 file changed, 52 insertions(+), 12 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acde4a1a50..20b77f1e27 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include "tests.h" static int mixer_callback(uintptr_t handle, @@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; +static bool should_prearm = false; + +#define NAN_VALUE 0.0f/0.0f int test_mixer(int argc, char *argv[]) { @@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[]) * PWM limit structure */ pwm_limit_t pwm_limit; - static bool should_arm = false; + bool should_arm = false; uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; @@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[]) const int jmax = 5; pwm_limit_init(&pwm_limit); - should_arm = true; /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -194,6 +198,35 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } + warnx("PRE-ARM TEST: DISABLING SAFETY"); + /* mix */ + should_prearm = true; + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (unsigned i = 0; i < mixed; i++) { + + warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + + if (i != actuator_controls_s::INDEX_THROTTLE) { + if (r_page_servos[i] < r_page_servo_control_min[i]) { + warnx("active servo < min"); + return 1; + } + } else { + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("throttle output != 0 (this check assumed the IO pass mixer!)"); + return 1; + } + } + } + + should_arm = true; + should_prearm = false; + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -205,11 +238,14 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -222,8 +258,6 @@ int test_mixer(int argc, char *argv[]) warnx("ramp servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -251,7 +285,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); @@ -278,18 +312,19 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -314,7 +349,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -324,6 +359,8 @@ int test_mixer(int argc, char *argv[]) /* check ramp */ + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || r_page_servos[i] > servo_predicted[i])) { @@ -338,8 +375,6 @@ int test_mixer(int argc, char *argv[]) warnx("mixer violated predicted value"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -397,5 +432,10 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; + if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + control = NAN_VALUE; + } + return 0; } From ef4946f81bb0ab897e4b7ade94d39cc8423fc632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:38:25 +0200 Subject: [PATCH 16/32] PWM limit: Avoid writing back into state struct --- src/modules/systemlib/pwm_limit/pwm_limit.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 09965b96b9..8f2cec6fc4 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -105,14 +105,17 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c * as the throttle channels need to go through the ramp at * regular arming time. */ + + unsigned local_limit_state = limit->state; + if (pre_armed) { - limit->state = PWM_LIMIT_STATE_ON; + local_limit_state = PWM_LIMIT_STATE_ON; } unsigned progress; /* then set effective_pwm based on state */ - switch (limit->state) { + switch (local_limit_state) { case PWM_LIMIT_STATE_OFF: case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:47:55 +0200 Subject: [PATCH 17/32] Tests: Reset mixer inputs --- src/systemcmds/tests/test_mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 20b77f1e27..3b2f42b21d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -227,6 +227,11 @@ int test_mixer(int argc, char *argv[]) should_arm = true; should_prearm = false; + /* simulate another orb_copy() from actuator controls */ + for (unsigned i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + } + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -249,7 +254,7 @@ int test_mixer(int argc, char *argv[]) /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + warnx("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]); return 1; } From 7b14a0258e8edefbc5b5eda1a86df925fa0d039d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:07 +0200 Subject: [PATCH 18/32] pwm limit: Fix author list --- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 8f2cec6fc4..cf71d7e335 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -37,6 +37,7 @@ * Library for PWM output limiting * * @author Julian Oes + * @author Lorenz Meier */ #include "pwm_limit.h" From 87b801034fdc897095bcdc959ae31225f3ed805b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:44 +0200 Subject: [PATCH 19/32] IO firmware: Fix condition for output enable to also allow no throttle arming to enable outputs --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 050750d080..1fa327613e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -281,7 +281,7 @@ mixer_tick(void) isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed && should_arm) { + if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); From 1795962328ffdaed093495409d09ad4bf374bf8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:11:54 +0200 Subject: [PATCH 20/32] Default Skywalker mixer to wing wing gains --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 22 +++++++++----------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3d464a4ae9..4950c3183f 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -14,18 +14,16 @@ then param set FW_AIRSPD_MAX 40 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 1 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + 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_RR_FF 0.6 + param set FW_RR_P 0.04 fi set MIXER X5 From c05c5bfceb70c6947d9f35c234d68187647af91b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:12:23 +0200 Subject: [PATCH 21/32] Multicopters: Load gimbal mixer by default --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index a5c326ebc6..6506ed8c3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -20,3 +20,11 @@ set PWM_RATE 400 set PWM_DISARMED 900 set PWM_MIN 1075 set PWM_MAX 2000 + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 From 3fa7006576a0aa2b013b56ca7e2c7f1d83f32cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 15:51:44 +0200 Subject: [PATCH 22/32] FW configs: Enable pass mixer by default --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index b718f421f5..f1b92112a3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -23,3 +23,11 @@ then param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0002 fi + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 From 396db730a6e6fdd72fc7a1a3dae1d8591b0d568f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 23:58:11 +0200 Subject: [PATCH 23/32] FMUv1: Enable PX4 FLOW driver --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 2e4829958e..26ee983ced 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -37,6 +37,7 @@ MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors +MODULES += drivers/px4flow # # System commands From 4465c029f54e1c412848a54bdfacc6431c6b3680 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Jul 2015 09:50:36 +0200 Subject: [PATCH 24/32] commander: Add new params for home initialization. --- src/modules/commander/commander_params.c | 76 ++++++++++++++++++++++-- 1 file changed, 70 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6e8e22b5cd..10284ab250 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -37,16 +36,54 @@ * * Parameters defined by the sensors task. * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #include #include +/** + * Roll trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); + +/** + * Pitch trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + */ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); + +/** + * Yaw trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); /** @@ -77,6 +114,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * * @group Battery Calibration * @unit V + * @min 0.0f */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -87,6 +125,8 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * * @group Battery Calibration * @unit S + * @min 1 + * @max 10 */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -182,7 +222,31 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @min 0 * @max 35 */ -PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); + +/** + * Home set horizontal threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit meter + * @min 2 + * @max 15 + */ +PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); + +/** + * Home set vertical threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit meter + * @min 5 + * @max 25 + */ +PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); /** * Autosaving of params From fc5b8ee12d28016a50947cdb67e93e340ead37b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Jul 2015 09:50:56 +0200 Subject: [PATCH 25/32] Commander: Use new params for home set thresholds --- src/modules/commander/commander.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f6fa5e6813..f7ef1ec8e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -870,6 +870,8 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_autostart_id = param_find("SYS_AUTOSTART"); param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); + param_t _param_eph = param_find("COM_HOME_H_T"); + param_t _param_epv = param_find("COM_HOME_V_T"); const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1293,6 +1295,10 @@ int commander_thread_main(int argc, char *argv[]) /* Parameter autosave setting */ param_get(_param_autosave_params, &autosave_params); + + /* EPH / EPV */ + param_get(_param_eph, &eph_threshold); + param_get(_param_epv, &epv_threshold); } /* Set flag to autosave parameters if necessary */ From 614c397762eddb986242c81f2baabaf412c8a379 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jul 2015 22:08:36 +0200 Subject: [PATCH 26/32] MPU6K self-test: Be more forgiving on offset values --- src/drivers/mpu6000/mpu6000.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9880544ecb..86fe334345 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -913,10 +913,14 @@ MPU6000::gyro_self_test() /* * Maximum deviation of 20 degrees, according to - * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * http://www.farnell.com/datasheets/1788002.pdf * Section 6.1, initial ZRO tolerance + * + * 20 dps (0.34 rad/s) initial offset + * and 20 dps temperature drift, so 0.34 rad/s * 2 */ - const float max_offset = 0.34f; + const float max_offset = 2.0f * 0.34f; + /* 30% scale error is chosen to catch completely faulty units but * to let some slight scale error pass. Requires a rate table or correlation * with mag rotations + data fit to From 9b6259c64f102287ec301b15a281f5fbcb0e7b10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jul 2015 15:51:20 +0200 Subject: [PATCH 27/32] Header generator: Only get active for no output files or when output older than input --- Tools/px_generate_uorb_topic_headers.py | 34 ++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 4f821b0608..6be0e19163 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -93,6 +93,29 @@ def convert_dir(inputdir, outputdir, templatedir): """ Converts all .msg files in inputdir to uORB header files """ + + # Find the most recent modification time in input dir + maxinputtime = 0 + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxinputtime: + maxinputtime = it; + + # Find the most recent modification time in output dir + maxouttime = 0 + for f in os.listdir(outputdir): + fni = os.path.join(outputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxouttime: + maxouttime = it; + + # Do not generate if nothing changed on the input + if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): + return False + includepath = incl_default + [':'.join([package, inputdir])] for f in os.listdir(inputdir): fn = os.path.join(inputdir, f) @@ -103,6 +126,8 @@ def convert_dir(inputdir, outputdir, templatedir): templatedir, includepath) + return True + def copy_changed(inputdir, outputdir, prefix=''): """ @@ -139,10 +164,11 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): Unchanged existing files are not overwritten. """ # Create new headers in temporary output directory - convert_dir(inputdir, temporarydir, templatedir) - - # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir, prefix) + if (convert_dir(inputdir, temporarydir, templatedir)): + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir, prefix) + else: + print('No changes.') if __name__ == "__main__": parser = argparse.ArgumentParser( From cdc17f1fdd8dfa3c315bd907dab8ca14f3f0cf8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jul 2015 15:51:43 +0200 Subject: [PATCH 28/32] Keep temporary topic files to avoid constant re-generation, but delete them on clean --- Makefile | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index aedf595aef..804e1c3c3c 100644 --- a/Makefile +++ b/Makefile @@ -275,6 +275,7 @@ TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages MULTIPLATFORM_PREFIX = px4_ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary +MULTI_TOPICHEADER_TEMP_DIR = $(BUILD_DIR)multi_topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src @@ -287,9 +288,7 @@ generateuorbtopicheaders: checksubmodules @$(ECHO) "Generating multiplatform uORB topic wrapper headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) -# clean up temporary files - $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(MULTI_TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # # Testing targets @@ -320,6 +319,8 @@ clean: $(Q) $(RMDIR) $(BUILD_DIR)*.build $(Q) $(RMDIR) $(PX4_VERSIONING_DIR) $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 + $(Q) $(RMDIR) $(TOPICHEADER_TEMP_DIR) + $(Q) $(RMDIR) $(MULTI_TOPICHEADER_TEMP_DIR) .PHONY: distclean distclean: clean From 17471eab65194bdc83549d195d4f15c4688fbe6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jul 2015 17:13:31 +0200 Subject: [PATCH 29/32] Topic generation: Accept non-existing output directory --- Tools/px_generate_uorb_topic_headers.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 6be0e19163..f1b2ff9ae0 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -105,12 +105,13 @@ def convert_dir(inputdir, outputdir, templatedir): # Find the most recent modification time in output dir maxouttime = 0 - for f in os.listdir(outputdir): - fni = os.path.join(outputdir, f) - if os.path.isfile(fni): - it = os.path.getmtime(fni) - if it > maxouttime: - maxouttime = it; + if os.path.isdir(outputdir) + for f in os.listdir(outputdir): + fni = os.path.join(outputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxouttime: + maxouttime = it; # Do not generate if nothing changed on the input if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): From 3c3abc078f1735a10a389fa8ac576a0db4821940 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jul 2015 17:23:11 +0200 Subject: [PATCH 30/32] Topic generation: Fix compile error --- Tools/px_generate_uorb_topic_headers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index f1b2ff9ae0..92d7dc1ef1 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -105,7 +105,7 @@ def convert_dir(inputdir, outputdir, templatedir): # Find the most recent modification time in output dir maxouttime = 0 - if os.path.isdir(outputdir) + if os.path.isdir(outputdir): for f in os.listdir(outputdir): fni = os.path.join(outputdir, f) if os.path.isfile(fni): From 9efc37a3ad672d88f76f2c82289bbc479251e7d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jul 2015 20:13:05 +0200 Subject: [PATCH 31/32] Fix topic generation logic for different board targets --- Tools/px_generate_uorb_topic_headers.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 92d7dc1ef1..221aa3ae38 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -149,14 +149,16 @@ def copy_changed(inputdir, outputdir, prefix=''): shutil.copy(fni, fno) print("{0}: new header file".format(f)) continue - # The file exists in inputdir and outputdir - # only copy if contents do not match - if not filecmp.cmp(fni, fno): - shutil.copy(fni, fno) - print("{0}: updated".format(f)) - continue - print("{0}: unchanged".format(f)) + if os.path.getmtime(fni) > os.path.getmtime(fno): + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + print("{0}: updated".format(f)) + continue + + #print("{0}: unchanged".format(f)) def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): @@ -165,11 +167,9 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): Unchanged existing files are not overwritten. """ # Create new headers in temporary output directory - if (convert_dir(inputdir, temporarydir, templatedir)): - # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir, prefix) - else: - print('No changes.') + convert_dir(inputdir, temporarydir, templatedir) + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir, prefix) if __name__ == "__main__": parser = argparse.ArgumentParser( From 482641e40313eb1a7e810021c72b0610d3b7882c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jul 2015 09:36:14 +0200 Subject: [PATCH 32/32] MAVLink: Crank onboard attitude rate up to 250 Hz --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1f43031be2..91c9ce7db3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1455,7 +1455,7 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 50.0f); + configure_stream("ATTITUDE", 250.0f); configure_stream("HIGHRES_IMU", 50.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f);