From f174ca3ce5dfe338b79f52de46f127abf1c3aca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 21:52:10 +0100 Subject: Added average as direct output --- src/modules/systemlib/perf_counter.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index bf84b7945..b4ca0ed3e 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, + pce->time_total / pce->event_count, pce->time_least, pce->time_most); break; -- cgit v1.2.3 From 0f0dc5ba068d24fb8b339acc8ef850f5f6ea9e47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 12:45:04 +0100 Subject: Allowed custom battery scaling on IO --- src/drivers/px4io/px4io.cpp | 17 +++++++++++++++-- src/modules/sensors/sensor_params.c | 1 + 2 files changed, 16 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9e84bf929..df96fa0bb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -896,8 +896,21 @@ PX4IO::task_main() /* re-upload RC input config as it may have changed */ io_set_rc_config(); - } - } + + /* re-set the battery scaling */ + int32_t voltage_scaling_val; + param_t voltage_scaling_param; + + /* see if bind parameter has been set, and reset it to -1 */ + param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); + + /* send channel config to IO */ + uint16_t scaling = voltage_scaling_val; + int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); + + if (pret != OK) { + log("voltage scaling upload failed"); + } perf_end(_perf_update); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2aa15420a..78d4b410a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -195,6 +195,7 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -- cgit v1.2.3 From 64ad3d7e0a60e994f6f26c18d52f4b2fd8b8beb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Dec 2013 18:44:07 +0100 Subject: Added channel count to log format --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2adb13f5c..e94b1e13c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90093a407..ab4dc9b00 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,6 +159,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t channel_count; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -281,7 +282,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), + LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), -- cgit v1.2.3 From a5023329920d5ce45c5bf48ae61d621947cdb349 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:10:24 +0100 Subject: Greatly robustified PPM parsing, needs cross-checking with receiver models --- src/drivers/stm32/drv_hrt.c | 39 ++++++++++++++++++++++++++++---------- src/modules/systemlib/ppm_decode.h | 1 + 2 files changed, 30 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index f105251f0..5bfbe04b8 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -282,6 +282,10 @@ static void hrt_call_invoke(void); * 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,10 +336,10 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */ +# define PPM_MAX_PULSE_WIDTH 700 /* 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_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 @@ -345,6 +349,7 @@ static void hrt_call_invoke(void); #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; @@ -362,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; struct { uint16_t last_edge; /* last capture time */ uint16_t last_mark; /* last significant edge */ - unsigned next_channel; + uint16_t frame_start; /* the frame width */ + unsigned next_channel; /* next channel index */ enum { UNSYNCH = 0, ARM, @@ -447,7 +453,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 +496,7 @@ hrt_ppm_decode(uint32_t status) ppm_buffer[i] = ppm_temp_buffer[i]; ppm_last_valid_decode = hrt_absolute_time(); + } } @@ -500,13 +506,14 @@ 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: @@ -515,14 +522,23 @@ hrt_ppm_decode(uint32_t status) goto error; /* pulse was 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_MAX_PULSE_WIDTH) + goto error; /* pulse was 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 +559,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: diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h index 6c5e15345..5a1ad84da 100644 --- a/src/modules/systemlib/ppm_decode.h +++ b/src/modules/systemlib/ppm_decode.h @@ -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 */ -- cgit v1.2.3 From edffade8cec2ea779040e97fb5478e0e9db12031 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:11:48 +0100 Subject: Added PPM frame length feedback in IO comms and status command - allows to warn users about badly formatted PPM frames --- src/drivers/px4io/px4io.cpp | 10 ++++++++++ src/modules/px4iofirmware/controls.c | 10 +++++++--- src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 6 ++++-- 4 files changed, 23 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b80844c5b..010272c6f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1724,6 +1724,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); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 194d8aab9..58af77997 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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; @@ -125,7 +125,7 @@ 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 */ @@ -319,7 +319,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; @@ -343,6 +343,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); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 11e380397..500e0ed4b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,7 +124,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 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3f9e111ba..6aa3a5cd2 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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 }; /** @@ -114,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** -- cgit v1.2.3 From 8d2950561d1889ab1d4c2fc5d832a2984048487d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:15:15 +0100 Subject: Changed RSSI range to 0..255 --- src/drivers/drv_rc_input.h | 2 +- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/sbus.c | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 7b18b5b15..66771faaa 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -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 */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58af77997..ed29c8339 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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); @@ -129,7 +129,7 @@ controls_tick() { if (ppm_updated) { /* XXX sample RSSI properly here */ - rssi = 1000; + rssi = 255; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 388502b40..3dcfe7f5b 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -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; } -- cgit v1.2.3