APM_RC: added OutputCh_current() method

this allows logging of the actual servo output values. The radio_out
method previously used doesn't take account of the various override
mechanisms available via waypoints

libraries/APM_RC/APM_RC.h

@@ -36,6 +36,7 @@ class APM_RC_Class
 
 	virtual void Init( Arduino_Mega_ISR_Registry * isr_reg ) = 0;
 	virtual void OutputCh(uint8_t ch, uint16_t pwm) = 0;
+	virtual uint16_t OutputCh_current(uint8_t ch) = 0;
 	virtual uint16_t InputCh(uint8_t ch) = 0;
 	virtual uint8_t GetState() = 0;
 	virtual void clearOverride(void) = 0;

libraries/APM_RC/APM_RC_APM1.cpp

@@ -157,6 +157,25 @@ void APM_RC_APM1::OutputCh(uint8_t ch, uint16_t pwm)
   }
 }
 
+uint16_t APM_RC_APM1::OutputCh_current(uint8_t ch)
+{
+	uint16_t pwm=0;
+	switch(ch) {
+	case 0:  pwm=OCR5B; break;  //ch1
+	case 1:  pwm=OCR5C; break;  //ch2
+	case 2:  pwm=OCR1B; break;  //ch3
+	case 3:  pwm=OCR1C; break;  //ch4
+	case 4:  pwm=OCR4C; break;  //ch5
+	case 5:  pwm=OCR4B; break;  //ch6
+	case 6:  pwm=OCR3C; break;  //ch7
+	case 7:  pwm=OCR3B; break;  //ch8
+	case 8:  pwm=OCR5A; break;  //ch9,  PL3
+	case 9:  pwm=OCR1A; break;  //ch10, PB5
+	case 10: pwm=OCR3A; break;  //ch11, PE3
+	}
+	return pwm>>1;
+}
+
 void APM_RC_APM1::enable_out(uint8_t ch)
 {
  switch(ch){

libraries/APM_RC/APM_RC_APM1.h

@@ -13,6 +13,7 @@ class APM_RC_APM1 : public APM_RC_Class
 	APM_RC_APM1();
 	void Init( Arduino_Mega_ISR_Registry * isr_reg );
 	void OutputCh(uint8_t ch, uint16_t pwm);
+	uint16_t OutputCh_current(uint8_t ch);
 	uint16_t InputCh(uint8_t ch);
 	uint8_t GetState();
 	bool setHIL(int16_t v[NUM_CHANNELS]);

libraries/APM_RC/APM_RC_APM2.cpp

@@ -161,6 +161,24 @@ void APM_RC_APM2::OutputCh(unsigned char ch, uint16_t pwm)
   }
 }
 
+uint16_t APM_RC_APM2::OutputCh_current(uint8_t ch)
+{
+	uint16_t pwm=0;
+	switch(ch){
+	case 0:  pwm=OCR1B; break;  // out1
+	case 1:  pwm=OCR1A; break;  // out2
+	case 2:  pwm=OCR4C; break;  // out3
+	case 3:  pwm=OCR4B; break;  // out4
+	case 4:  pwm=OCR4A; break;  // out5
+	case 5:  pwm=OCR3C; break;  // out6
+	case 6:  pwm=OCR3B; break;  // out7
+	case 7:  pwm=OCR3A; break;  // out8
+	case 9:  pwm=OCR5B; break;  // out10
+	case 10: pwm=OCR5C; break;  // out11
+	}
+	return pwm>>1;
+}
+
 void APM_RC_APM2::enable_out(uint8_t ch)
 {
   switch(ch) {

libraries/APM_RC/APM_RC_APM2.h

@@ -14,7 +14,8 @@ class APM_RC_APM2 : public APM_RC_Class
   public:
 	APM_RC_APM2();
 	void Init( Arduino_Mega_ISR_Registry * isr_reg );
-	void OutputCh(unsigned char ch, uint16_t pwm);
+	void OutputCh(uint8_t ch, uint16_t pwm);
+	uint16_t OutputCh_current(uint8_t ch);
 	uint16_t InputCh(unsigned char ch);
 	unsigned char GetState();
 	bool setHIL(int16_t v[NUM_CHANNELS]);