aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNosDE <marco@wtns.de>2015-02-16 22:06:17 +0100
committerLorenz Meier <lm@inf.ethz.ch>2015-02-18 16:55:05 +0100
commit5fddb89c3e3290df26822c59c11fbfd4f7981f16 (patch)
treeebcef8ec5c827ce24660e26978d1bdc920d4fc08
parentbd2d411a09a5d9747ed5ea3ef75173f44b33d997 (diff)
downloadpx4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.tar.gz
px4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.tar.bz2
px4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.zip
mkblctrl - rework and bugfix
-rw-r--r--ROMFS/px4fmu_common/init.d/rcS4
-rw-r--r--src/drivers/drv_tone_alarm.h1
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp275
-rw-r--r--src/drivers/mkblctrl/mkblctrl_params.c52
-rw-r--r--src/drivers/mkblctrl/module.mk3
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp2
6 files changed, 197 insertions, 140 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8b26bbe7a..927e7f99e 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -333,11 +333,11 @@ then
if [ $OUTPUT_MODE == mkblctrl ]
then
set MKBLCTRL_ARG ""
- if [ $MK_MODE == x ]
+ if [ $MKBLCTRL_MODE == x ]
then
set MKBLCTRL_ARG "-mkmode x"
fi
- if [ $MK_MODE == + ]
+ if [ $MKBLCTRL_MODE == + ]
then
set MKBLCTRL_ARG "-mkmode +"
fi
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index acfcecca1..31aee266e 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -151,6 +151,7 @@ enum {
TONE_PARACHUTE_RELEASE_TUNE,
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
+ TONE_SINGLE_BEEP_TUNE,
TONE_NUMBER_OF_TUNES
};
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1331744ae..f3db4dd94 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
@@ -42,8 +42,8 @@
*/
#include <nuttx/config.h>
-
#include <drivers/device/i2c.h>
+#include <systemlib/param/param.h>
#include <sys/types.h>
#include <stdint.h>
@@ -67,11 +67,12 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
+#include <drivers/drv_mixer.h>
+#include <drivers/drv_tone_alarm.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
@@ -81,17 +82,18 @@
#include <systemlib/err.h>
-#define I2C_BUS_SPEED 400000
-#define UPDATE_RATE 400
+#define I2C_BUS_SPEED 100000
+#define UPDATE_RATE 200
#define MAX_MOTORS 8
#define BLCTRL_BASE_ADDR 0x29
#define BLCTRL_OLD 0
#define BLCTRL_NEW 1
#define BLCTRL_MIN_VALUE -0.920F
#define MOTOR_STATE_PRESENT_MASK 0x80
-#define MOTOR_STATE_ERROR_MASK 0x7F
+#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 30
-#define ESC_UORB_PUBLISH_DELAY 500000
+#define MOTOR_LOCATE_DELAY 10000000
+#define ESC_UORB_PUBLISH_DELAY 500000
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
@@ -108,6 +110,9 @@ struct MotorData_t {
unsigned int RoundCount;
};
+
+
+
class MK : public device::I2C
{
public:
@@ -128,7 +133,6 @@ public:
virtual int init(unsigned motors);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
- int set_update_rate(unsigned rate);
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
@@ -141,7 +145,6 @@ private:
static const bool showDebug = false;
int _update_rate;
- int _current_update_rate;
int _task;
int _t_actuators;
int _t_actuator_armed;
@@ -179,20 +182,17 @@ private:
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
- struct GPIOConfig {
- uint32_t input;
- uint32_t output;
- uint32_t alt;
- };
-
- static const GPIOConfig _gpio_tab[];
- static const unsigned _ngpio;
int mk_servo_arm(bool status);
int mk_servo_set(unsigned int chan, short val);
- int mk_servo_set_value(unsigned int chan, short val);
int mk_servo_test(unsigned int chan);
+ int mk_servo_locate();
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
+
+ void play_beep(int count);
+ bool _indicate_esc;
+ param_t _param_indicate_esc;
+
};
@@ -218,8 +218,8 @@ MK *g_mk;
} // namespace
MK::MK(int bus, const char *_device_path) :
- I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
- _update_rate(400),
+ I2C("mkblctrl", "/dev/mkblctrl0", bus, 0, I2C_BUS_SPEED),
+ _update_rate(UPDATE_RATE),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
@@ -234,13 +234,15 @@ MK::MK(int bus, const char *_device_path) :
_overrideSecurityChecks(false),
_task_should_exit(false),
_armed(false),
- _mixers(nullptr)
+ _mixers(nullptr),
+ _indicate_esc(false)
{
strncpy(_device, _device_path, sizeof(_device));
/* enforce null termination */
_device[sizeof(_device) - 1] = '\0';
_debug_enabled = true;
+
}
MK::~MK()
@@ -274,6 +276,8 @@ MK::~MK()
int
MK::init(unsigned motors)
{
+ _param_indicate_esc = param_find("MKBLCTRL_TEST");
+
_num_outputs = motors;
debugCounter = 0;
int ret;
@@ -321,16 +325,6 @@ MK::task_main_trampoline(int argc, char *argv[])
g_mk->task_main();
}
-int
-MK::set_update_rate(unsigned rate)
-{
- if ((rate > 500) || (rate < 10))
- return -EINVAL;
-
- _update_rate = rate;
- return OK;
-}
-
void
MK::set_px4mode(int px4mode)
{
@@ -439,16 +433,28 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
}
void
+MK::play_beep(int count)
+{
+ int buzzer = ::open(TONEALARM0_DEVICE_PATH, O_WRONLY);
+
+ for (int i = 0; i < count; i++) {
+ ::ioctl(buzzer, TONE_SET_ALARM, TONE_SINGLE_BEEP_TUNE);
+ usleep(300000);
+ }
+
+ ::close(buzzer);
+}
+
+void
MK::task_main()
{
+ int32_t param_mkblctrl_test = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
- _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-
- /* force a reset of the update rate */
- _current_update_rate = 0;
+ _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0));
+ orb_set_interval(_t_actuators, 20); /* default to 50Hz */
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
@@ -467,37 +473,26 @@ MK::task_main()
_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
-
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
+
+ orb_set_interval(_t_actuators, int(1000 / _update_rate));
+ up_pwm_servo_set_rate(_update_rate);
+
log("starting");
/* loop until killed */
while (!_task_should_exit) {
- /* handle update rate changes */
- if (_current_update_rate != _update_rate) {
- int update_rate_in_ms = int(1000 / _update_rate);
-
- /* reject faster than 500 Hz updates */
- if (update_rate_in_ms < 2) {
- update_rate_in_ms = 2;
- _update_rate = 500;
- }
-
- /* reject slower than 50 Hz updates */
- if (update_rate_in_ms > 20) {
- update_rate_in_ms = 20;
- _update_rate = 50;
- }
-
- orb_set_interval(_t_actuators, update_rate_in_ms);
- up_pwm_servo_set_rate(_update_rate);
- _current_update_rate = _update_rate;
+ param_get(_param_indicate_esc ,&param_mkblctrl_test);
+ if (param_mkblctrl_test > 0) {
+ _indicate_esc = true;
+ } else {
+ _indicate_esc = false;
}
/* sleep waiting for data max 100ms */
@@ -513,66 +508,69 @@ MK::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
- /* get controls - must always do this to avoid spinning */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+ bool changed = false;
+ orb_check(_t_actuators, &changed);
- /* can we mix? */
- if (_mixers != nullptr) {
+ if (changed) {
- /* do mixing */
- outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
- outputs.timestamp = hrt_absolute_time();
+ /* get controls - must always do this to avoid spinning */
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
- /* iterate actuators */
- for (unsigned int i = 0; i < _num_outputs; i++) {
+ /* can we mix? */
+ if (_mixers != nullptr) {
- /* last resort: catch NaN, INF and out-of-band errors */
- if (i < outputs.noutputs &&
- isfinite(outputs.output[i]) &&
- outputs.output[i] >= -1.0f &&
- outputs.output[i] <= 1.0f) {
- /* scale for PWM output 900 - 2100us */
- /* nothing to do here */
- } else {
- /*
- * 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.
- */
- if (outputs.output[i] < -1.0f) {
- outputs.output[i] = -1.0f;
+ /* do mixing */
+ outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
+ outputs.timestamp = hrt_absolute_time();
- } else if (outputs.output[i] > 1.0f) {
- outputs.output[i] = 1.0f;
+ /* iterate actuators */
+ for (unsigned int i = 0; i < _num_outputs; i++) {
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ /* nothing to do here */
} else {
- outputs.output[i] = -1.0f;
+ /*
+ * 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.
+ */
+ if (outputs.output[i] < -1.0f) {
+ outputs.output[i] = -1.0f;
+
+ } else if (outputs.output[i] > 1.0f) {
+ outputs.output[i] = 1.0f;
+
+ } else {
+ outputs.output[i] = -1.0f;
+ }
}
- }
- if (!_overrideSecurityChecks) {
- /* don't go under BLCTRL_MIN_VALUE */
+ if (!_overrideSecurityChecks) {
+ /* don't go under BLCTRL_MIN_VALUE */
+
+ if (outputs.output[i] < BLCTRL_MIN_VALUE) {
+ outputs.output[i] = BLCTRL_MIN_VALUE;
+ }
- if (outputs.output[i] < BLCTRL_MIN_VALUE) {
- outputs.output[i] = BLCTRL_MIN_VALUE;
}
- }
+ /* output to BLCtrl's */
+ if (_motortest != true && _indicate_esc != true) {
+ Motor[i].SetPoint_PX4 = outputs.output[i];
+ mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine
+ }
- /* output to BLCtrl's */
- if (_motortest != true) {
- //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
- // 11 Bit
- Motor[i].SetPoint_PX4 = outputs.output[i];
- mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine
}
}
-
}
-
}
/* how about an arming update? */
@@ -624,6 +622,11 @@ MK::task_main()
if (_motortest == true) {
mk_servo_test(i);
}
+
+ // if esc locate is requested
+ if (_indicate_esc == true) {
+ mk_servo_locate();
+ }
}
@@ -834,39 +837,6 @@ MK::mk_servo_set(unsigned int chan, short val)
return 0;
}
-int
-MK::mk_servo_set_value(unsigned int chan, short val)
-{
- _retries = 0;
- int ret;
- short tmpVal = 0;
- uint8_t msg[2] = { 0, 0 };
-
- tmpVal = val;
-
- if (tmpVal > 1024) {
- tmpVal = 1024;
-
- } else if (tmpVal < 0) {
- tmpVal = 0;
- }
-
- Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
-
- if (_armed == false) {
- Motor[chan].SetPoint = 0;
- Motor[chan].SetPointLowerBits = 0;
- }
-
- msg[0] = Motor[chan].SetPoint;
-
- set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
- ret = transfer(&msg[0], 1, nullptr, 0);
-
- ret = OK;
- return ret;
-}
-
int
MK::mk_servo_test(unsigned int chan)
@@ -936,6 +906,39 @@ MK::mk_servo_test(unsigned int chan)
int
+MK::mk_servo_locate()
+{
+ int ret = 0;
+ static unsigned int chan = 0;
+ static uint64_t last_timestamp = 0;
+ _retries = 0;
+ uint8_t msg[2] = { 0, 0 };
+
+
+ if (hrt_absolute_time() - last_timestamp > MOTOR_LOCATE_DELAY) {
+ last_timestamp = hrt_absolute_time();
+
+ set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+ chan++;
+
+ if (chan <= _num_outputs) {
+ fprintf(stderr, "[mkblctrl] ESC Locate - #%i:\tgreen\n", chan);
+ play_beep(chan);
+ }
+
+ if (chan > _num_outputs) {
+ chan = 0;
+ }
+ }
+
+ // do i2c transfer to selected esc
+ ret = transfer(&msg[0], 1, nullptr, 0);
+
+ return ret;
+}
+
+
+int
MK::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
@@ -1130,7 +1133,7 @@ enum FrameType {
int
-mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
+mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
{
int shouldStop = 0;
@@ -1160,8 +1163,6 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
- g_mk->set_update_rate(update_rate);
-
return OK;
}
@@ -1178,7 +1179,7 @@ mk_start(unsigned motors, const char *device_path)
if (OK == g_mk->init(motors)) {
warnx("[mkblctrl] scanning i2c3...\n");
- ret = g_mk->mk_check_for_blctrl(8, false, true);
+ ret = g_mk->mk_check_for_blctrl(8, false, false);
if (ret > 0) {
return OK;
@@ -1196,7 +1197,7 @@ mk_start(unsigned motors, const char *device_path)
if (OK == g_mk->init(motors)) {
warnx("[mkblctrl] scanning i2c1...\n");
- ret = g_mk->mk_check_for_blctrl(8, false, true);
+ ret = g_mk->mk_check_for_blctrl(8, false, false);
if (ret > 0) {
return OK;
@@ -1217,14 +1218,13 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
int
mkblctrl_main(int argc, char *argv[])
{
- int pwm_update_rate_in_hz = UPDATE_RATE;
int motorcount = 8;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
bool overrideSecurityChecks = false;
bool showHelp = false;
- bool newMode = false;
+ bool newMode = true;
const char *devicepath = "";
/*
@@ -1265,6 +1265,7 @@ mkblctrl_main(int argc, char *argv[])
/* look for the optional -h --help parameter */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
showHelp = true;
+ newMode = false;
}
/* look for the optional --override-security-checks parameter */
@@ -1311,7 +1312,7 @@ mkblctrl_main(int argc, char *argv[])
/* parameter set ? */
if (newMode) {
/* switch parameter */
- return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+ return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
exit(0);
diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c
new file mode 100644
index 000000000..4296260f8
--- /dev/null
+++ b/src/drivers/mkblctrl/mkblctrl_params.c
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ * Author: Marco Bauer <marco@wtns.de>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mkblctrl_params.c
+ *
+ * Parameters defined by the mkblctrl driver.
+ *
+ * @author Marco Bauer <marco@wtns.de>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Enables testmode (Identify) of MKBLCTRL Driver
+ *
+ * @group MKBLCTRL Testmode
+ */
+PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0);
+
diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk
index 6daa14aa5..10468509e 100644
--- a/src/drivers/mkblctrl/module.mk
+++ b/src/drivers/mkblctrl/module.mk
@@ -37,7 +37,8 @@
MODULE_COMMAND = mkblctrl
-SRCS = mkblctrl.cpp
+SRCS = mkblctrl.cpp \
+ mkblctrl_params.c
INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index ae533e811..8b6847348 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -338,6 +338,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
+ _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -352,6 +353,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
+ _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep
}
ToneAlarm::~ToneAlarm()