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

Merged beta into master

This commit is contained in:
Lorenz Meier 2015-07-09 15:55:31 +02:00
parent b1b555ceb6 3fa7006576
commit 2a8402edb1
15 changed files with 326 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

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

8
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

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

8
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

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

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

@ -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();
@ -242,6 +245,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;
actuator_armed_s PX4FMU::_armed = {};
namespace
{
@ -261,7 +265,6 @@ PX4FMU::PX4FMU() :
_armed_sub(-1),
_param_sub(-1),
_outputs_pub(nullptr),
_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;
}

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

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

2
src/modules/mavlink/mavlink.c
View File

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

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

@ -35,6 +35,8 @@
* @file mixer.cpp
*
* Control channel input/output mixer and failsafe.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_config.h>
@ -60,10 +62,12 @@ 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;
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 +176,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);
@ -235,26 +244,27 @@ 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);
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.0f;
}
/* 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 */
@ -271,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]);
@ -308,8 +318,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 +370,15 @@ mixer_callback(uintptr_t handle,
}
}
/* 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) {
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,
@ -370,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;
@ -458,8 +478,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 +503,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 +523,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;
}
}

24
src/modules/systemlib/mixer/mixer.cpp
View File

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

24
src/modules/systemlib/mixer/mixer.h
View File

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

16
src/modules/systemlib/mixer/mixer_load.c
View File

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

90
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -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;
@ -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,54 +253,59 @@ 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 +
@ -304,26 +314,28 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
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 +

40
src/modules/systemlib/mixer/mixer_simple.cpp
View File

@ -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
@ -79,6 +80,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
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,9 +343,10 @@ SimpleMixer::check()
/* validate the scaler */
ret = scale_check(_info->controls[i].scaler);
if (ret != 0)
if (ret != 0) {
return (10 * i + ret);
}
}
return 0;
}

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

@ -37,6 +37,7 @@
* Library for PWM output limiting
*
* @author Julian Oes <julian@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "pwm_limit.h"
@ -54,7 +55,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,10 +100,23 @@ 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.
*/
unsigned local_limit_state = limit->state;
if (pre_armed) {
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<num_channels; i++) {
@ -121,6 +135,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
for (unsigned i=0; i<num_channels; i++) {
float control_value = output[i];
/* check for invalid / disabled channels */
if (!isfinite(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
continue;
}
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
@ -141,8 +163,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
ramp_min_pwm = min_pwm[i];
}
float control_value = output[i];
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}
@ -163,6 +183,12 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
float control_value = output[i];
/* check for invalid / disabled channels */
if (!isfinite(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
continue;
}
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}

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

@ -71,7 +71,7 @@ typedef struct {
__EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
__EXPORT 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);
__END_DECLS

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

@ -56,6 +56,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_controls.h>
#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,40 @@ int test_mixer(int argc, char *argv[])
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
PX4_INFO("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;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
@ -205,15 +243,18 @@ 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]) {
PX4_ERR("disarmed servo value mismatch");
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return 1;
}
@ -222,21 +263,16 @@ int test_mixer(int argc, char *argv[])
PX4_ERR("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);
sleepcount++;
if (sleepcount % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\n");
PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
@ -251,7 +287,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);
PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
@ -278,18 +314,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]) {
PX4_ERR("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 +351,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 +361,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 +377,6 @@ int test_mixer(int argc, char *argv[])
PX4_ERR("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 +434,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;
}