fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 01:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed pwm limit command to behave as originally designed. The initial hold time produced random values (e.g. 40000 instead of 1500) during the INIT_TIME (0.5s) phase

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-02 09:18:04 +01:00
parent eb47a164cb
commit a60fcc2535
2 changed files with 55 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pwm_limit/pwm_limit.c
+51 -33
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch> * Author: Julian Oes <joes@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <stdio.h>
void pwm_limit_init(pwm_limit_t *limit) void pwm_limit_init(pwm_limit_t *limit)
{ {
limit->state = PWM_LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_INIT;
limit->time_armed = 0; limit->time_armed = 0;
return; return;
} }
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) void pwm_limit_calc(const bool armed, const unsigned num_channels, 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)
{ {
/* first evaluate state changes */ /* first evaluate state changes */
switch (limit->state) { switch (limit->state) {
case PWM_LIMIT_STATE_OFF:
if (armed)
limit->state = PWM_LIMIT_STATE_RAMP;
limit->time_armed = hrt_absolute_time();
break;
case PWM_LIMIT_STATE_INIT: case PWM_LIMIT_STATE_INIT:
if (!armed)
limit->state = PWM_LIMIT_STATE_OFF; if (armed) {
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
/* set arming time for the first call */
if (limit->time_armed == 0) {
limit->time_armed = hrt_absolute_time();
}
if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF;
}
}
break;
case PWM_LIMIT_STATE_OFF:
if (armed) {
limit->state = PWM_LIMIT_STATE_RAMP; limit->state = PWM_LIMIT_STATE_RAMP;
/* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time();
}
break; break;
case PWM_LIMIT_STATE_RAMP: case PWM_LIMIT_STATE_RAMP:
if (!armed) if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON; limit->state = PWM_LIMIT_STATE_ON;
}
break; break;
case PWM_LIMIT_STATE_ON: case PWM_LIMIT_STATE_ON:
if (!armed) if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF; limit->state = PWM_LIMIT_STATE_OFF;
}
break; break;
default: default:
break; break;
@@ -90,40 +105,43 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
case PWM_LIMIT_STATE_INIT: case PWM_LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i]; effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
} }
break; break;
case PWM_LIMIT_STATE_RAMP: case PWM_LIMIT_STATE_RAMP:
{
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; progress = diff * 10000 / RAMP_TIME_US;
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm; for (unsigned i=0; i<num_channels; i++) {
/* if a disarmed pwm value was set, blend between disarmed and min */ uint16_t ramp_min_pwm;
if (disarmed_pwm[i] > 0) {
/* safeguard against overflows */ /* if a disarmed pwm value was set, blend between disarmed and min */
uint16_t disarmed = disarmed_pwm[i]; if (disarmed_pwm[i] > 0) {
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
uint16_t disarmed_min_diff = min_pwm[i] - disarmed; /* safeguard against overflows */
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; unsigned disarmed = disarmed_pwm[i];
} else { if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i];
}
/* no disarmed pwm value set, choose min pwm */ unsigned disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = min_pwm[i]; ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
} }
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
output[i] = (float)progress/10000.0f * output[i];
} }
break; break;
case PWM_LIMIT_STATE_ON: case PWM_LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* effective_output stays the same */
} }
break; break;
default: default:
src/modules/systemlib/pwm_limit/pwm_limit.h
+1 -1
View File
@@ -72,7 +72,7 @@ typedef struct {
__EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, 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);
__END_DECLS __END_DECLS