fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'px4io-i2c-throttle' into px4io-i2c

Browse Source
This commit is contained in:
Lorenz Meier 2013-03-07 11:46:39 +01:00
commit bafddddfbc
7 changed files with 116 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
apps/drivers/px4io/px4io.cpp
View File

@ -83,6 +83,7 @@
#include <px4io/protocol.h>
#include "uploader.h"
#include <debug.h>
class PX4IO : public device::I2C
@ -771,9 +772,16 @@ PX4IO::io_set_rc_config()
/* send channel config to IO */
ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
if (ret != OK) {
log("RC config update failed");
log("rc config upload failed");
break;
}
/* check the IO initialisation flag */
if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
log("config for RC%d rejected by IO", i + 1);
break;
}
offset += PX4IO_P_RC_CONFIG_STRIDE;
}
@ -1186,7 +1194,7 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@ -1197,7 +1205,8 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"));
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
printf("alarms 0x%04x%s%s%s%s%s%s\n",
alarms,

46
apps/px4io/controls.c
View File

@ -147,32 +147,44 @@ controls_tick() {
uint16_t raw = r_raw_rc_values[i];
/* implement the deadzone */
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
}
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
}
int16_t scaled;
/* constrain to min/max values */
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
int16_t scaled = raw;
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/* adjust to zero-relative around center (nominal -500..500) */
scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
/* scale to fixed-point representation (-10000..10000) */
scaled *= 20;
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;

8
apps/px4io/mixer.cpp
View File

@ -170,9 +170,11 @@ mixer_tick(void)
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK));
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */

1
apps/px4io/protocol.h
View File

@ -103,6 +103,7 @@
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */

52
apps/px4io/registers.c
View File

@ -383,6 +383,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_RC_CONFIG_OPTIONS:
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
@ -390,27 +391,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* should the channel be enabled? */
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
break;
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
break;
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
break;
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
break;
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
count++;
}
/* assert deadzone is sane */
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
break;
if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
break;
if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
break;
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
break;
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
// The following check isn't needed as constraint checks in controls.c will catch this.
//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
// count++;
//}
//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
// count++;
//}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
count++;
}
/* sanity checks pass, enable channel */
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
break;
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */

7
apps/px4io/safety.c
View File

@ -176,12 +176,17 @@ heartbeat_blink(void *arg)
static void
failsafe_blink(void *arg)
{
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
failsafe = !failsafe;
} else {
failsafe = false;
}

42
apps/sensors/sensors.cpp
View File

@ -1125,31 +1125,45 @@ Sensors::ppm_poll()
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
/* scale around the mid point differently for lower and upper range */
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (rc_input.values[i] < _parameters.min[i])
rc_input.values[i] = _parameters.min[i];
if (rc_input.values[i] > _parameters.max[i])
rc_input.values[i] = _parameters.max[i];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* the total range is 2 (-1..1).
* If center (trim) == min, scale to 0..1, if center (trim) == max,
* scale to -1..0.
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
}
/* reverse channel if required */
if (i == (int)_rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
_rc.chan[i].scaled *= _parameters.rev[i];
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
if (!isfinite(_rc.chan[i].scaled))
_rc.chan[i].scaled = 0.0f;
}