Merge remote-tracking branch 'px4/master' into navigator_new

This commit is contained in:
Julian Oes
2013-12-29 14:48:54 +01:00
16 changed files with 477 additions and 98 deletions

View File

@@ -89,7 +89,7 @@ struct rc_input_values {
/** number of channels actually being seen */
uint32_t channel_count;
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
int32_t rssi;
/** Input source */

View File

@@ -850,7 +850,7 @@ PX4IO::task_main()
/* we're not nice to the lower-priority control groups and only check them
when the primary group updated (which is now). */
io_set_control_groups();
(void)io_set_control_groups();
}
if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -962,14 +962,14 @@ out:
int
PX4IO::io_set_control_groups()
{
bool attitude_ok = io_set_control_state(0);
int ret = io_set_control_state(0);
/* send auxiliary control groups */
(void)io_set_control_state(1);
(void)io_set_control_state(2);
(void)io_set_control_state(3);
return attitude_ok;
return ret;
}
int
@@ -1095,8 +1095,10 @@ PX4IO::io_set_rc_config()
* assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
* controls.
*/
/* fill the mapping with an error condition triggering value */
for (unsigned i = 0; i < _max_rc_input; i++)
input_map[i] = -1;
input_map[i] = UINT8_MAX;
/*
* NOTE: The indices for mapped channels are 1-based
@@ -1128,12 +1130,6 @@ PX4IO::io_set_rc_config()
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 4;
ichan = 5;
for (unsigned i = 0; i < _max_rc_input; i++)
if (input_map[i] == -1)
input_map[i] = ichan++;
/*
* Iterate all possible RC inputs.
*/
@@ -1801,6 +1797,16 @@ PX4IO::print_status()
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
printf("\n");
if (raw_inputs > 0) {
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n");
}
}
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
printf("mapped R/C inputs 0x%04x", mapped_inputs);

View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -168,7 +168,7 @@
# error HRT_TIMER_CLOCK must be greater than 1MHz
#endif
/*
/**
* Minimum/maximum deadlines.
*
* These are suitable for use with a 16-bit timer/counter clocked
@@ -276,12 +276,16 @@ static void hrt_call_invoke(void);
* Specific registers and bits used by PPM sub-functions
*/
#ifdef HRT_PPM_CHANNEL
/*
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
*/
# ifdef CONFIG_ARCH_CORTEXM3
# undef GTIM_CCER_CC1NP
# undef GTIM_CCER_CC2NP
# undef GTIM_CCER_CC3NP
# undef GTIM_CCER_CC4NP
# define GTIM_CCER_CC1NP 0
# define GTIM_CCER_CC2NP 0
# define GTIM_CCER_CC3NP 0
@@ -332,19 +336,21 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */
# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */
# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */
# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */
/* decoded PPM buffer */
#define PPM_MIN_CHANNELS 5
#define PPM_MAX_CHANNELS 20
/* Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */
/** Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
__EXPORT uint16_t ppm_frame_length = 0;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
@@ -358,11 +364,12 @@ unsigned ppm_pulse_next;
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
/* PPM decoder state machine */
/** PPM decoder state machine */
struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
unsigned next_channel;
uint16_t last_edge; /**< last capture time */
uint16_t last_mark; /**< last significant edge */
uint16_t frame_start; /**< the frame width */
unsigned next_channel; /**< next channel index */
enum {
UNSYNCH = 0,
ARM,
@@ -384,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status);
# define CCER_PPM 0
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Initialise the timer we are going to use.
*
* We expect that we'll own one of the reduced-function STM32 general
@@ -430,7 +437,7 @@ hrt_tim_init(void)
}
#ifdef HRT_PPM_CHANNEL
/*
/**
* Handle the PPM decoder state machine.
*/
static void
@@ -447,7 +454,6 @@ hrt_ppm_decode(uint32_t status)
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
ppm.last_edge = count;
ppm_edge_history[ppm_edge_next++] = width;
@@ -491,6 +497,7 @@ hrt_ppm_decode(uint32_t status)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
@@ -500,29 +507,39 @@ hrt_ppm_decode(uint32_t status)
/* next edge is the reference for the first channel */
ppm.phase = ARM;
ppm.last_edge = count;
return;
}
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
return;
break;
case ARM:
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = count;
ppm.phase = INACTIVE;
return;
ppm.last_mark = ppm.last_edge;
/* frame length is everything including the start gap */
ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
ppm.frame_start = ppm.last_edge;
ppm.phase = ACTIVE;
break;
case INACTIVE:
/* we expect a short pulse */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
return;
break;
case ACTIVE:
/* determine the interval from the last mark */
@@ -543,10 +560,13 @@ hrt_ppm_decode(uint32_t status)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
return;
break;
}
ppm.last_edge = count;
return;
/* the state machine is corrupted; reset it */
error:
@@ -557,7 +577,7 @@ error:
}
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
@@ -586,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
hrt_ppm_decode(status);
}
#endif
/* was this a timer tick? */
@@ -604,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
return OK;
}
/*
/**
* Fetch a never-wrapping absolute time value in microseconds from
* some arbitrary epoch shortly after system start.
*/
@@ -651,7 +672,7 @@ hrt_absolute_time(void)
return abstime;
}
/*
/**
* Convert a timespec to absolute time
*/
hrt_abstime
@@ -665,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
return result;
}
/*
/**
* Convert absolute time to a timespec.
*/
void
@@ -676,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
/*
/**
* Compare a time value with the current time.
*/
hrt_abstime
@@ -691,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
return delta;
}
/*
/**
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
@@ -706,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
return ts;
}
/*
/**
* Initalise the high-resolution timing module.
*/
void
@@ -721,7 +742,7 @@ hrt_init(void)
#endif
}
/*
/**
* Call callout(arg) after interval has elapsed.
*/
void
@@ -734,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
arg);
}
/*
/**
* Call callout(arg) at calltime.
*/
void
@@ -743,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
hrt_call_internal(entry, calltime, 0, callout, arg);
}
/*
/**
* Call callout(arg) every period.
*/
void
@@ -762,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
@@ -782,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqrestore(flags);
}
/*
/**
* If this returns true, the call has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
@@ -793,7 +814,7 @@ hrt_called(struct hrt_call *entry)
return (entry->deadline == 0);
}
/*
/**
* Remove the entry from the callout list.
*/
void
@@ -876,17 +897,18 @@ hrt_call_invoke(void)
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
}
/*
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.

View File

@@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/* accel measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/* mag measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
@@ -72,10 +74,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V3_R0");
h->r1 = param_find("EKF_ATT_V3_R1");
h->r2 = param_find("EKF_ATT_V3_R2");
h->r3 = param_find("EKF_ATT_V3_R3");
h->r0 = param_find("EKF_ATT_V4_R0");
h->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");

View File

@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
static bool ppm_input(uint16_t *values, uint16_t *num_values);
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -94,7 +94,7 @@ controls_tick() {
* other. Don't do that.
*/
/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
perf_begin(c_gather_dsm);
@@ -108,7 +108,7 @@ controls_tick() {
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
rssi = 1000;
rssi = 255;
}
perf_end(c_gather_dsm);
@@ -125,11 +125,11 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
rssi = 1000;
rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
@@ -321,7 +321,7 @@ controls_tick() {
}
static bool
ppm_input(uint16_t *values, uint16_t *num_values)
ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -345,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
/* store PPM frame length */
if (num_values)
*frame_len = ppm_frame_length;
/* good if we got any channels */
result = (*num_values > 0);
}

View File

@@ -128,7 +128,8 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */

View File

@@ -89,7 +89,9 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_IBATT] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0
[PX4IO_P_STATUS_PRSSI] = 0,
[PX4IO_P_STATUS_NRSSI] = 0,
[PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -582,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -600,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -608,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
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 {
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}

View File

@@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
}
*rssi = 1000;
*rssi = 255;
return true;
}

View File

@@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);

View File

@@ -57,6 +57,7 @@ __BEGIN_DECLS
* PPM decoder state
*/
__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */
__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */
__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */