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

Merge branch 'px4io-i2c-throttle' of github.com:PX4/Firmware into px4io-i2c-throttle

This commit is contained in:
Lorenz Meier 2013-03-07 09:53:37 +01:00
parent 84f9599cb3 4050a05de7
commit 5ab8ea9226
4 changed files with 41 additions and 29 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,

45
apps/px4io/controls.c
View File

@ -147,32 +147,43 @@ 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 center is min 0..20000, if center is max -20000..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 = 20.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 = 20.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;

3
apps/px4io/px4io.c
View File

@ -153,9 +153,6 @@ user_start(int argc, char *argv[])
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* initialise the registry space */
registers_init();
/* initialise the control inputs */
controls_init();

7
apps/px4io/registers.c
View File

@ -179,12 +179,6 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
*/
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
void
registers_init(void)
{
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@ -389,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;