From fd63ba7b898b95469b62f6f3f42f12e042aae619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jul 2015 01:43:00 +0200 Subject: [PATCH 01/14] 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 b5436602ec9..8856dd829dd 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 02/14] 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 0106fa1eb79..a2cfdf491ba 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 3ab41c5c58f..cd2010b92cc 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 1190683015d..cd981419056 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 0d629d61007..c0950d77a75 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 aa0a8e74186..6bbc349d87f 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 e48bda69181..5c2edef61b6 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 03/14] 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 b1d369cbf4f..df2a5786adf 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 04/14] 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 2047046b9b3..b1766f7390e 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 05/14] 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 a2cfdf491ba..050750d0807 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 06/14] 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 2f72d347c60..09965b96b9d 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 07/14] 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 acde4a1a50e..20b77f1e27e 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 08/14] 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 09965b96b9d..8f2cec6fc44 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 09/14] 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 20b77f1e27e..3b2f42b21d9 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 10/14] 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 8f2cec6fc44..cf71d7e335e 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 11/14] 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 050750d0807..1fa327613ea 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 12/14] 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 3d464a4ae97..4950c3183f9 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 13/14] 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 a5c326ebc60..6506ed8c3e5 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 14/14] 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 b718f421f5b..f1b92112a33 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