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

mkblctrl - rework and bugfix

This commit is contained in:
NosDE 2015-02-16 22:06:17 +01:00 committed by Lorenz Meier
parent bd2d411a09
commit 5fddb89c3e
6 changed files with 198 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rcS
View File

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

1
src/drivers/drv_tone_alarm.h
View File

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

277
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2015 PX4 Development Team. All rights reserved.
* 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)
{
@ -438,17 +432,29 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
return retVal;
}
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;
if (!_overrideSecurityChecks) {
/* don't go under BLCTRL_MIN_VALUE */
} else if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
} else {
outputs.output[i] = -1.0f;
}
}
}
if (!_overrideSecurityChecks) {
/* don't go under 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)
@ -935,6 +905,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,
@ -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);

52
src/drivers/mkblctrl/mkblctrl_params.c Normal file
View File

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

3
src/drivers/mkblctrl/module.mk
View File

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

2
src/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

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