forked from Archive/PX4-Autopilot
Added functionality to enable PWM output for stupid ESCs even when safety is not off, arming button functionality remains as is
This commit is contained in:
Julian Oes
parent
ece93ab628
commit
23858a6726
|
|
@ -136,6 +136,11 @@ public:
|
|||
*/
|
||||
int set_max_values(const uint16_t *vals, unsigned len);
|
||||
|
||||
/**
|
||||
* Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
|
||||
*/
|
||||
int set_idle_values(const uint16_t *vals, unsigned len);
|
||||
|
||||
/**
|
||||
* Print the current status of IO
|
||||
*/
|
||||
|
|
@ -567,7 +572,8 @@ PX4IO::init()
|
|||
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
||||
PX4IO_P_SETUP_ARMING_FMU_ARMED |
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
||||
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
|
||||
|
||||
/* publish RC config to IO */
|
||||
ret = io_set_rc_config();
|
||||
|
|
@ -793,6 +799,20 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len)
|
|||
return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
|
||||
{
|
||||
uint16_t regs[_max_actuators];
|
||||
|
||||
if (len > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
|
||||
printf("Sending IDLE values\n");
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
|
|
@ -1441,12 +1461,14 @@ PX4IO::print_status()
|
|||
/* setup and state */
|
||||
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
|
||||
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
|
||||
printf("arming 0x%04x%s%s%s%s\n",
|
||||
printf("arming 0x%04x%s%s%s%s%s%s\n",
|
||||
arming,
|
||||
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
|
||||
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
|
||||
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
|
||||
|
|
@ -1473,8 +1495,10 @@ PX4IO::print_status()
|
|||
}
|
||||
printf("failsafe");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
|
||||
printf("\n");
|
||||
printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
|
||||
printf("idle values");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -1973,6 +1997,41 @@ px4io_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "idle")) {
|
||||
|
||||
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 0. */
|
||||
uint16_t idle[8];
|
||||
|
||||
for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
|
||||
{
|
||||
/* set channel to commanline argument or to 0 for non-provided channels */
|
||||
if (argc > i + 2) {
|
||||
idle[i] = atoi(argv[i+2]);
|
||||
if (idle[i] < 900 || idle[i] > 2100) {
|
||||
errx(1, "value out of range of 900 < value < 2100. Aborting.");
|
||||
}
|
||||
} else {
|
||||
/* a zero value will the default */
|
||||
idle[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
|
||||
|
||||
if (ret != OK)
|
||||
errx(ret, "failed setting idle values");
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "recovery")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
|
|
@ -2100,5 +2159,5 @@ px4io_main(int argc, char *argv[])
|
|||
monitor();
|
||||
|
||||
out:
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@ extern "C" {
|
|||
* Time that the ESCs need to initialize
|
||||
*/
|
||||
#define ESC_INIT_TIME_US 1000000
|
||||
#define ESC_RAMP_TIME_US 2000000
|
||||
|
||||
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
|
||||
#define ROLL 0
|
||||
|
|
@ -73,10 +74,17 @@ extern "C" {
|
|||
|
||||
/* current servo arm/disarm state */
|
||||
static bool mixer_servos_armed = false;
|
||||
|
||||
static bool should_arm = false;
|
||||
static bool should_always_enable_pwm = false;
|
||||
static uint64_t esc_init_time;
|
||||
static bool esc_init_active = false;
|
||||
static bool esc_init_done = false;
|
||||
|
||||
enum esc_state_e {
|
||||
ESC_OFF,
|
||||
ESC_INIT,
|
||||
ESC_RAMP,
|
||||
ESC_ON
|
||||
};
|
||||
static esc_state_e esc_state;
|
||||
|
||||
/* selected control values and count for mixing */
|
||||
enum mixer_source {
|
||||
|
|
@ -175,18 +183,48 @@ mixer_tick(void)
|
|||
float outputs[IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
/* after arming, some ESCs need an initalization period, count the time from here */
|
||||
if (mixer_servos_armed && !esc_init_done && !esc_init_active) {
|
||||
esc_init_time = hrt_absolute_time();
|
||||
esc_init_active = true;
|
||||
uint16_t ramp_promille;
|
||||
|
||||
/* update esc init state, but only if we are truely armed and not just PWM enabled */
|
||||
if (mixer_servos_armed && should_arm) {
|
||||
|
||||
switch (esc_state) {
|
||||
|
||||
/* after arming, some ESCs need an initalization period, count the time from here */
|
||||
case ESC_OFF:
|
||||
esc_init_time = hrt_absolute_time();
|
||||
esc_state = ESC_INIT;
|
||||
break;
|
||||
|
||||
/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
|
||||
case ESC_INIT:
|
||||
if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
|
||||
esc_state = ESC_RAMP;
|
||||
}
|
||||
break;
|
||||
|
||||
/* then ramp until the min speed is reached */
|
||||
case ESC_RAMP:
|
||||
if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
|
||||
esc_state = ESC_ON;
|
||||
}
|
||||
break;
|
||||
|
||||
case ESC_ON:
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
esc_state = ESC_OFF;
|
||||
}
|
||||
|
||||
/* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */
|
||||
if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) {
|
||||
esc_init_active = false;
|
||||
esc_init_done = true;
|
||||
/* do the calculations during the ramp for all at once */
|
||||
if(esc_state == ESC_RAMP) {
|
||||
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
|
||||
}
|
||||
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
|
||||
|
|
@ -196,21 +234,27 @@ mixer_tick(void)
|
|||
/* save actuator values for FMU readback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
|
||||
/* XXX maybe this check for an armed FMU could be achieved a little less messy */
|
||||
if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
}
|
||||
/* during ESC initialization, use low PWM */
|
||||
else if (esc_init_active) {
|
||||
r_page_servos[i] = (outputs[i] * 600 + 1500);
|
||||
|
||||
/* afterwards use min and max values */
|
||||
} else {
|
||||
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);
|
||||
}
|
||||
switch (esc_state) {
|
||||
case ESC_INIT:
|
||||
r_page_servos[i] = (outputs[i] * 600 + 1500);
|
||||
break;
|
||||
|
||||
case ESC_RAMP:
|
||||
r_page_servos[i] = (outputs[i]
|
||||
* (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
|
||||
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
|
||||
break;
|
||||
|
||||
case ESC_ON:
|
||||
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);
|
||||
break;
|
||||
|
||||
case ESC_OFF:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
r_page_servos[i] = 0;
|
||||
|
|
@ -225,36 +269,43 @@ mixer_tick(void)
|
|||
* XXX correct behaviour for failsafe may require an additional case
|
||||
* here.
|
||||
*/
|
||||
bool should_arm = (
|
||||
should_arm = (
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
/* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) ||
|
||||
|
||||
/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) )
|
||||
/* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
/* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) ||
|
||||
/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) )
|
||||
);
|
||||
|
||||
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);
|
||||
|
||||
if (should_arm && !mixer_servos_armed) {
|
||||
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
|
||||
/* need to arm, but not armed */
|
||||
up_pwm_servo_arm(true);
|
||||
mixer_servos_armed = true;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
|
||||
isr_debug(5, "> armed");
|
||||
isr_debug(5, "> PWM enabled");
|
||||
|
||||
} else if (!should_arm && mixer_servos_armed) {
|
||||
} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
|
||||
/* armed but need to disarm */
|
||||
up_pwm_servo_arm(false);
|
||||
mixer_servos_armed = false;
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
|
||||
isr_debug(5, "> disarmed");
|
||||
isr_debug(5, "> PWM disabled");
|
||||
|
||||
esc_init_active = false; }
|
||||
}
|
||||
|
||||
if (mixer_servos_armed) {
|
||||
if (mixer_servos_armed && should_arm) {
|
||||
/* update the servo outputs. */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
up_pwm_servo_set(i, r_page_servos[i]);
|
||||
|
||||
} else if (mixer_servos_armed && should_always_enable_pwm) {
|
||||
/* set the idle servo outputs. */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
up_pwm_servo_set(i, r_page_servo_idle[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -155,6 +155,7 @@
|
|||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
|
||||
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
|
||||
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
|
||||
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
|
||||
|
||||
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
|
||||
|
|
@ -190,9 +191,12 @@
|
|||
/* PWM minimum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM maximum values for certain ESCs */
|
||||
/* PWM maximum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM idle values that are active, even when SAFETY_SAFE */
|
||||
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/**
|
||||
* As-needed mixer data upload.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@ 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 */
|
||||
extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
|
||||
|
||||
/*
|
||||
* Register aliases.
|
||||
|
|
|
|||
|
|
@ -147,7 +147,8 @@ volatile uint16_t r_page_setup[] =
|
|||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
|
||||
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
|
||||
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
|
||||
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
|
|
@ -201,6 +202,14 @@ uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900,
|
|||
*/
|
||||
uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
|
||||
|
||||
/**
|
||||
* PAGE 108
|
||||
*
|
||||
* idle PWM values for difficult ESCs
|
||||
*
|
||||
*/
|
||||
uint16_t r_page_servo_idle[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
|
||||
|
||||
void
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
{
|
||||
|
|
@ -308,6 +317,31 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
}
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_IDLE_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0)
|
||||
/* set to default */
|
||||
r_page_servo_idle[offset] = 0;
|
||||
|
||||
else if (*values < 900)
|
||||
r_page_servo_idle[offset] = 900;
|
||||
else if (*values > 2100)
|
||||
r_page_servo_idle[offset] = 2100;
|
||||
else
|
||||
r_page_servo_idle[offset] = *values;
|
||||
|
||||
/* flag the failsafe values as custom */
|
||||
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
break;
|
||||
|
||||
/* handle text going to the mixer parser */
|
||||
case PX4IO_PAGE_MIXERLOAD:
|
||||
mixer_handle_text(values, num_values * sizeof(*values));
|
||||
|
|
@ -651,6 +685,9 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
case PX4IO_PAGE_CONTROL_MAX_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_max);
|
||||
break;
|
||||
case PX4IO_PAGE_IDLE_PWM:
|
||||
SELECT_PAGE(r_page_servo_idle);
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
|
|
|
|||
Loading…
Reference in New Issue