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

Changed pwm_limit interface a bit

This commit is contained in:
Julian Oes 2013-10-11 10:19:50 +02:00
parent e2fef6b374
commit 3dc2bdfa22
3 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/px4iofirmware/mixer.cpp
View File

@ -197,9 +197,7 @@ mixer_tick(void)
/* mix */ /* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
pwm_limit.nchannels = mixed; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0; r_page_servos[i] = 0;

19
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -45,15 +45,14 @@
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
__EXPORT void pwm_limit_init(pwm_limit_t *limit) void pwm_limit_init(pwm_limit_t *limit)
{ {
limit->nchannels = 0;
limit->state = LIMIT_STATE_OFF; limit->state = LIMIT_STATE_OFF;
limit->time_armed = 0; limit->time_armed = 0;
return; return;
} }
__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, 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, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{ {
/* first evaluate state changes */ /* first evaluate state changes */
switch (limit->state) { switch (limit->state) {
@ -89,24 +88,26 @@ __EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, con
switch (limit->state) { switch (limit->state) {
case LIMIT_STATE_OFF: case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT: case LIMIT_STATE_INIT:
for (unsigned i=0; i<limit->nchannels; 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 LIMIT_STATE_RAMP: case LIMIT_STATE_RAMP:
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
for (unsigned i=0; i<limit->nchannels; i++) { for (unsigned i=0; i<num_channels; i++) {
temp_pwm = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; temp_pwm = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* already follow user/controller input if higher than min_pwm */ /* already follow user/controller input if higher than min_pwm */
effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000; effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000;
output[i] = (float)progress/10000.0f * output[i];
} }
break; break;
case LIMIT_STATE_ON: case LIMIT_STATE_ON:
for (unsigned i=0; i<limit->nchannels; i++) { for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output_requested[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:

8
src/modules/systemlib/pwm_limit/pwm_limit.h
View File

@ -46,8 +46,6 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
__BEGIN_DECLS
/* /*
* time for the ESCs to initialize * time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot) * (this is not actually needed if PWM is sent right after boot)
@ -66,13 +64,13 @@ typedef struct {
LIMIT_STATE_ON LIMIT_STATE_ON
} state; } state;
uint64_t time_armed; uint64_t time_armed;
unsigned nchannels;
} pwm_limit_t; } pwm_limit_t;
__BEGIN_DECLS
__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 uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, 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, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS __END_DECLS