Merge branch 'mathlib_new' into vector_control2

9 files changed, 24 insertions, 12 deletions

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 194d8aab9..ed29c8339 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;
@@ -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;
 	}
@@ -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
 };
 
 /**
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;
 }
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index bbdef3677..da7586de0 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1208,6 +1208,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 2bbde18fc..6bfc8606e 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 --- */
@@ -291,7 +292,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"),
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
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;
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 */