fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 07:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

[PX4IO/PWM driver] Added trim values to the PWM output drivers

Browse Source
This commit is contained in:
Bartosz Wawrzacz
2016-07-06 13:50:08 +02:00
committed by Lorenz Meier
parent 4494182bfc
commit 619efa7b45
12 changed files with 222 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -249,7 +249,7 @@ mixer_tick(void)
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_min, r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
src/modules/px4iofirmware/protocol.h
+4 -1
View File
@@ -298,8 +298,11 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM mtrim values for central position */
#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
src/modules/px4iofirmware/px4io.h
+1
View File
@@ -90,6 +90,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_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
src/modules/px4iofirmware/registers.c
+38
View File
@@ -260,6 +260,14 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_D
/**
* PAGE 108
*
* trim PWM values for center position
*
*/
uint16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM };
/**
* PAGE 109
*
* disarmed PWM values for difficult ESCs
*
*/
@@ -386,6 +394,32 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
break;
case PX4IO_PAGE_CONTROL_TRIM_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* allow 0 - turns the trim option off */
r_page_servo_control_trim[offset] = 0;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_trim[offset] = PWM_HIGHEST_MAX;
} else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_trim[offset] = PWM_LOWEST_MAX;
} else {
r_page_servo_control_trim[offset] = *values;
}
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_DISARMED_PWM: {
/* flag for all outputs */
bool all_disarmed_off = true;
@@ -1000,6 +1034,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_CONTROL_TRIM_PWM:
SELECT_PAGE(r_page_servo_control_trim);
break;
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
src/modules/systemlib/pwm_limit/pwm_limit.c
+10 -2
View File
@@ -56,7 +56,7 @@ void pwm_limit_init(pwm_limit_t *limit)
}
void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{
@@ -206,7 +206,15 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
control_value = -1.0f * control_value;
}
effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
if (trim_pwm[i] == 0) {
effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
} else if (control_value < 0) {
effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i];
} else {
effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i];
}
/* last line of defense against invalid inputs */
if (effective_pwm[i] < min_pwm[i]) {
src/modules/systemlib/pwm_limit/pwm_limit.h
+2 -1
View File
@@ -73,7 +73,8 @@ __EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels,
const uint16_t reverse_mask, 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);
const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS