diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 414eb06ddb..66e12325d2 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,5 +1,11 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf0d5e8d6f..6f13853fbe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2015 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 @@ -155,7 +155,7 @@ private: pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - pwm_limit_t _pwm_limit; + static pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -241,6 +241,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; namespace { @@ -272,7 +273,6 @@ PX4FMU::PX4FMU() : _control_subs{-1}, _actuator_output_topic_instance(-1), _poll_fds_num(0), - _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -827,6 +827,18 @@ PX4FMU::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; input = controls[control_group].control[control_index]; + + /* motor spinup phase - lock throttle to zero */ + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + 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, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + return 0; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1490232a4f..5391f7769a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -100,6 +100,14 @@ bool MulticopterLandDetector::update() return true; } + // return status based on armed state if no position lock is available + if (_vehicleGlobalPosition.timestamp == 0 || + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + + // no position lock - not landed if armed + return !_arming.armed; + } + const uint64_t now = hrt_absolute_time(); // check if we are moving vertically diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index f182495ac3..77bac2ad74 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -43,29 +43,29 @@ /** * Multicopter max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f); /** * Multicopter max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f); /** * Multicopter max rotation * - * Maximum allowed around each axis to trigger a land (degrees per second) + * Maximum allowed around each axis allowed in the landed state (degrees per second) * * @unit deg/s * @@ -76,19 +76,19 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); /** * Multicopter max throttle * - * Maximum actuator output on throttle before triggering a land + * Maximum actuator output on throttle allowed in the landed state * * @min 0.1 * @max 0.5 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); /** * Fixedwing max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @min 0.5 * @max 10 @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @min 5 * @max 20 @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * - * Maximum airspeed allowed to trigger a land (m/s) + * Maximum airspeed allowed in the landed state (m/s) * * @min 4 * @max 20 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1dc2c22d39..f42fa9fe4e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1417,7 +1417,11 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = math::min(_manual.z, _params.thr_max); - _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + + /* enforce minimum throttle if not landed */ + if (!_vehicle_status.condition_landed) { + _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + } } /* construct attitude setpoint rotation matrix */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 516eb59b86..f67717d5c7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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 @@ -49,6 +49,7 @@ #include #include +#include extern "C" { //#define DEBUG @@ -318,13 +319,6 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } return -1; @@ -333,13 +327,6 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); @@ -353,6 +340,33 @@ mixer_callback(uintptr_t handle, return -1; } + /* apply trim offsets for override channels */ + if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_ROLL) { + control += REG_TO_FLOAT(r_setup_trim_roll); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_PITCH) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_YAW) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } + } + + /* motor spinup phase - lock throttle to zero */ + if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + 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, + * as the motors cannot follow any demand yet + */ + control = 0.0f; + } + } + /* limit output */ if (control > 1.0f) { control = 1.0f; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index adcfb703c0..2f72d347c6 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: Julian Oes + * 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 @@ -35,9 +34,9 @@ /** * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #include "pwm_limit.h" @@ -46,6 +45,8 @@ #include #include +#define PROGRESS_INT_SCALING 10000 + void pwm_limit_init(pwm_limit_t *limit) { limit->state = PWM_LIMIT_STATE_INIT; @@ -112,7 +113,11 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ { hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = diff * 10000 / RAMP_TIME_US; + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; + + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; + } for (unsigned i=0; i + * 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 @@ -33,11 +32,11 @@ ****************************************************************************/ /** - * @file pwm_limit.h + * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #ifndef PWM_LIMIT_H_ diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 3f8e924db3..2d1d755288 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -200,7 +200,7 @@ int test_mixer(int argc, char *argv[]) hrt_abstime starttime = hrt_absolute_time(); unsigned sleepcount = 0; - while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) { /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); @@ -259,7 +259,7 @@ int test_mixer(int argc, char *argv[]) for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; - if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + if (abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); PX4_ERR("mixer violated predicted value"); return 1; @@ -333,7 +333,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); PX4_ERR("mixer violated predicted value"); return 1;