fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 11:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

First try to prevent motors from stopping when armed

Browse Source
This commit is contained in:
Julian Oes
2013-06-18 00:30:10 +02:00
parent 2daff9ebbf
commit cc452834c0
5 changed files with 192 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+108
View File
@@ -125,6 +125,16 @@ public:
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Set the minimum PWM signals when armed
*/
int set_min_values(const uint16_t *vals, unsigned len);
/**
* Set the maximum PWM signal when armed
*/
int set_max_values(const uint16_t *vals, unsigned len);
/**
* Print the current status of IO
*/
@@ -711,6 +721,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int
PX4IO::set_min_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
}
int
PX4IO::set_max_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
}
int
PX4IO::io_set_arming_state()
{
@@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "min")) {
if (argc < 3) {
errx(1, "min command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 900. */
uint16_t min[8];
for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
min[i] = atoi(argv[i+2]);
if (min[i] < 900 || min[i] > 1200) {
errx(1, "value out of range of 900 < value < 1200. Aborting.");
}
} else {
/* a zero value will the default */
min[i] = 900;
}
}
int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
if (ret != OK)
errx(ret, "failed setting min values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "max")) {
if (argc < 3) {
errx(1, "max command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 2100. */
uint16_t max[8];
for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
{
/* set channel to commanline argument or to 2100 for non-provided channels */
if (argc > i + 2) {
max[i] = atoi(argv[i+2]);
if (max[i] < 1800 || max[i] > 2100) {
errx(1, "value out of range of 1800 < value < 2100. Aborting.");
}
} else {
/* a zero value will the default */
max[i] = 2100;
}
}
int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
if (ret != OK)
errx(ret, "failed setting max values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
src/modules/px4iofirmware/mixer.cpp
+18 -2
View File
@@ -59,6 +59,11 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/*
* Time that the ESCs need to initialize
*/
#define ESC_INIT_TIME_US 500000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -69,6 +74,8 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static uint64_t time_armed;
/* selected control values and count for mixing */
enum mixer_source {
MIX_NONE,
@@ -176,8 +183,16 @@ mixer_tick(void)
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
/* scale to control range after init time */
if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) {
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
/* but use init range from 900 to 2100 right after arming */
} else {
r_page_servos[i] = (outputs[i] * 600 + 1500);
}
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@@ -206,6 +221,7 @@ mixer_tick(void)
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
time_armed = hrt_absolute_time();
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
src/modules/px4iofirmware/protocol.h
+6
View File
@@ -183,6 +183,12 @@
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM minimum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
*
src/modules/px4iofirmware/px4io.h
+2
View File
@@ -77,6 +77,8 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
/*
* Register aliases.
src/modules/px4iofirmware/registers.c
+58
View File
@@ -184,6 +184,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
*/
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
/**
* PAGE 106
*
* minimum PWM values when armed
*
*/
uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
/**
* PAGE 107
*
* maximum PWM values when armed
*
*/
uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -247,6 +263,42 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
/* copy channel data */
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
if (*values > 1200)
r_page_servo_control_min[offset] = 1200;
else if (*values < 900)
r_page_servo_control_min[offset] = 900;
else
r_page_servo_control_min[offset] = *values;
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
/* copy channel data */
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
if (*values > 1200)
r_page_servo_control_max[offset] = 1200;
else if (*values < 900)
r_page_servo_control_max[offset] = 900;
else
r_page_servo_control_max[offset] = *values;
offset++;
num_values--;
values++;
}
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
@@ -583,6 +635,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
SELECT_PAGE(r_page_servo_control_min);
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
default:
return -1;