dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2482 from PX4/mc_thr_lim 

MC pos control: Enforce minimum throttle in manual attitude control m…
This commit is contained in:
Lorenz Meier 2015-07-02 00:59:00 +02:00
parent 3ef6212155 6697ffb668
commit 10961aac0e
9 changed files with 91 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/actuator_controls.msg
View File

@ -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

18
src/drivers/px4fmu/fmu.cpp
View File

@ -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;
}

8
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -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

20
src/modules/land_detector/land_detector_params.c
View File

@ -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

6
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1414,7 +1414,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 */

44
src/modules/px4iofirmware/mixer.cpp
View File

@ -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 <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
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;

17
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
* 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 <joes@student.ethz.ch>
* @author Julian Oes <julian@px4.io>
*/
#include "pwm_limit.h"
@ -46,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <stdio.h>
#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<num_channels; i++) {
@ -128,7 +133,7 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
}
unsigned disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
} else {

9
src/modules/systemlib/pwm_limit/pwm_limit.h
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
* 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 <joes@student.ethz.ch>
* @author Julian Oes <julian@px4.io>
*/
#ifndef PWM_LIMIT_H_

6
src/systemcmds/tests/test_mixer.cpp
View File

@ -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]);
warnx("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]);
warnx("mixer violated predicted value");
return 1;