SITL: added RCInput and RCOutput

libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h

@@ -9,7 +9,9 @@ namespace AVR_SITL {
     class SITLConsoleDriver;
     class SITLEEPROMStorage;
     class SITLAnalogIn; 
-    class ADCSource;
+    class SITLRCInput;
+    class SITLRCOutput;
+   class ADCSource;
     class RCInput;
 }
 

libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

@@ -15,6 +15,7 @@
 #include "Storage.h"
 #include "Console.h"
 #include "RCInput.h"
+#include "RCOutput.h"
 #include "SITL_State.h"
 
 #include <AP_HAL_Empty.h>
@@ -26,9 +27,9 @@ static SITLScheduler sitlScheduler;
 static SITLEEPROMStorage sitlEEPROMStorage;
 static SITLConsoleDriver consoleDriver;
 static SITL_State sitlState;
+static SITLRCInput  sitlRCInput(&sitlState);
+static SITLRCOutput sitlRCOutput(&sitlState);
 
-static Empty::EmptyRCInput  emptyRCInput;
-static Empty::EmptyRCOutput emptyRCOutput;
 static Empty::EmptyGPIO emptyGPIO;
 static Empty::EmptyAnalogIn emptyAnalogIn;
 
@@ -46,11 +47,9 @@ HAL_AVR_SITL::HAL_AVR_SITL() :
         &emptyAnalogIn, /* analogin */
         &sitlEEPROMStorage, /* storage */
         &consoleDriver, /* console */
-
         &emptyGPIO, /* gpio */
-        &emptyRCInput,  /* rcinput */
+        &sitlRCInput,  /* rcinput */
-        &emptyRCOutput, /* rcoutput */
+        &sitlRCOutput, /* rcoutput */
-
         &sitlScheduler), /* scheduler */
     _sitl_state(&sitlState)
 {}

libraries/AP_HAL_AVR_SITL/RCInput.cpp

@@ -0,0 +1,52 @@
+
+#include "RCInput.h"
+
+using namespace AVR_SITL;
+
+extern const AP_HAL::HAL& hal;
+
+void SITLRCInput::init(void* machtnichts)
+{
+    clear_overrides();
+}
+
+uint8_t SITLRCInput::valid() {
+    return 0;
+}
+
+uint16_t SITLRCInput::read(uint8_t ch) {
+    return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
+}
+
+uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
+    for (uint8_t i=0; i<len; i++) {
+	periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
+    }
+    return len;
+}
+
+bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) {
+    bool res = false;
+    for (uint8_t i = 0; i < len; i++) {
+        res |= set_override(i, overrides[i]);
+    }
+    return res;
+}
+
+bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
+    if (override < 0) return false; /* -1: no change. */
+    if (channel < 8) {
+        _override[channel] = override;
+        if (override != 0) {
+            return true;
+        }
+    }
+    return false;
+}
+
+void SITLRCInput::clear_overrides()
+{
+    for (uint8_t i = 0; i < 8; i++) {
+	_override[i] = 0;
+    }
+}

libraries/AP_HAL_AVR_SITL/RCInput.h

@@ -0,0 +1,28 @@
+
+#ifndef __AP_HAL_AVR_SITL_RCINPUT_H__
+#define __AP_HAL_AVR_SITL_RCINPUT_H__
+
+#include <AP_HAL_AVR_SITL.h>
+
+class AVR_SITL::SITLRCInput : public AP_HAL::RCInput {
+public:
+    SITLRCInput(SITL_State *sitlState) {
+	    _sitlState = sitlState;
+    }
+    void init(void* machtnichts);
+    uint8_t  valid();
+    uint16_t read(uint8_t ch);
+    uint8_t read(uint16_t* periods, uint8_t len);
+
+    bool set_overrides(int16_t *overrides, uint8_t len);
+    bool set_override(uint8_t channel, int16_t override);
+    void clear_overrides();
+private:
+    SITL_State *_sitlState;
+
+    /* override state */
+    uint16_t _override[8];
+};
+
+#endif // __AP_HAL_AVR_SITL_RCINPUT_H__
+

libraries/AP_HAL_AVR_SITL/RCOutput.cpp

@@ -0,0 +1,45 @@
+
+#include "RCOutput.h"
+
+using namespace AVR_SITL;
+
+void SITLRCOutput::init(void* machtnichts) {}
+
+void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
+    _freq_hz = freq_hz;
+}
+
+uint16_t SITLRCOutput::get_freq(uint8_t ch) {
+    return _freq_hz;
+}
+
+void SITLRCOutput::enable_ch(uint8_t ch)
+{}
+
+void SITLRCOutput::enable_mask(uint32_t chmask)
+{}
+
+void SITLRCOutput::disable_ch(uint8_t ch)
+{}
+
+void SITLRCOutput::disable_mask(uint32_t chmask)
+{}
+
+void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
+{
+	_sitlState->pwm_output[ch] = period_us;
+}
+
+void SITLRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+	memcpy(_sitlState->pwm_output+ch, period_us, len*sizeof(uint16_t));
+}
+
+uint16_t SITLRCOutput::read(uint8_t ch) {
+	return _sitlState->pwm_output[ch];
+}
+
+void SITLRCOutput::read(uint16_t* period_us, uint8_t len)
+{
+	memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
+}

libraries/AP_HAL_AVR_SITL/RCOutput.h

@@ -0,0 +1,31 @@
+
+#ifndef __AP_HAL_AVR_SITL_RCOUTPUT_H__
+#define __AP_HAL_AVR_SITL_RCOUTPUT_H__
+
+#include <AP_HAL_AVR_SITL.h>
+
+class AVR_SITL::SITLRCOutput : public AP_HAL::RCOutput {
+public:
+    SITLRCOutput(SITL_State *sitlState) {
+	    _sitlState = sitlState;
+	    _freq_hz = 50;
+    }
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     enable_mask(uint32_t chmask);
+    void     disable_ch(uint8_t ch);
+    void     disable_mask(uint32_t chmask);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+
+private:    
+    SITL_State *_sitlState;
+    uint16_t _freq_hz;
+};
+
+#endif // __AP_HAL_AVR_SITL_RCOUTPUT_H__
+

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -22,9 +22,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-// emulate RC input
-struct RC_ICR4 ICR4;
-
 using namespace AVR_SITL;
 
 enum SITL_State::vehicle_type SITL_State::_vehicle;
@@ -41,7 +38,8 @@ AP_Compass_HIL *SITL_State::_compass;
 
 int SITL_State::_sitl_fd;
 SITL *SITL_State::_sitl;
-uint16_t SITL_State::_pwm_output[11];
+uint16_t SITL_State::pwm_output[11];
+uint16_t SITL_State::pwm_input[8];
 
 // catch floating point exceptions
 void SITL_State::_sig_fpe(int signum)
@@ -99,7 +97,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
 			_framerate = 50;
 		}
 		// set right default throttle for rover (allowing for reverse)
-		ICR4.set(2, 1500);
+        pwm_input[2] = 1500;
 	} else {
 		_vehicle = ArduPlane;
 		if (_framerate == 0) {
@@ -302,10 +300,9 @@ void SITL_State::_fdm_input(void)
 		// a packet giving the receiver PWM inputs
 		uint8_t i;
 		for (i=0; i<8; i++) {
-			// setup the ICR4 register for the RC channel
+			// setup the pwn input for the RC channel inputs
-			// inputs
 			if (d.pwm_pkt.pwm[i] != 0) {
-				ICR4.set(i, d.pwm_pkt.pwm[i]);
+				pwm_input[i] = d.pwm_pkt.pwm[i];
 			}
 		}
 		break;
@@ -331,15 +328,15 @@ void SITL_State::_simulator_output(void)
 
 	if (last_update == 0) {
 		for (i=0; i<11; i++) {
-			_pwm_output[i] = 1000;
+			pwm_output[i] = 1000;
 		}
 		if (_vehicle == ArduPlane) {
-			_pwm_output[0] = _pwm_output[1] = _pwm_output[3] = 1500;
+			pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500;
-			_pwm_output[7] = 1800;
+			pwm_output[7] = 1800;
 		}
 		if (_vehicle == APMrover2) {
-			_pwm_output[0] = _pwm_output[1] = _pwm_output[2] = _pwm_output[3] = 1500;
+			pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
-			_pwm_output[7] = 1800;
+			pwm_output[7] = 1800;
 		}
 	}
 
@@ -354,10 +351,10 @@ void SITL_State::_simulator_output(void)
 	last_update = hal.scheduler->millis();
 
 	for (i=0; i<11; i++) {
-		if (_pwm_output[i] == 0xFFFF) {
+		if (pwm_output[i] == 0xFFFF) {
 			control.pwm[i] = 0;
 		} else {
-			control.pwm[i] = _pwm_output[i];
+			control.pwm[i] = pwm_output[i];
 		}
 	}
 
@@ -445,6 +442,10 @@ Vector3f SITL_State::_rand_vec3f(void)
 
 void SITL_State::init(int argc, char * const argv[])
 {
+    pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500;
+    pwm_input[4] = pwm_input[7] = 1800;
+    pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
+
     _scheduler = (SITLScheduler *)hal.scheduler;
 	_parse_command_line(argc, argv);
 }

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -9,7 +9,6 @@
 #include <AP_HAL_AVR_SITL.h>
 #include "AP_HAL_AVR_SITL_Namespace.h"
 #include "HAL_AVR_SITL_Class.h"
-#include "sitl_rc.h"
 
 #include <sys/types.h>
 #include <sys/socket.h>
@@ -22,8 +21,6 @@
 #include "../AP_Compass/AP_Compass.h"
 #include "../SITL/SITL.h"
 
-extern struct RC_ICR4 ICR4;
-
 class HAL_AVR_SITL;
 
 class AVR_SITL::SITL_State {
@@ -38,6 +35,8 @@ public:
 
     int gps_pipe(void);
     ssize_t gps_read(int fd, void *buf, size_t count);
+    static uint16_t pwm_output[11];
+    static uint16_t pwm_input[8];
 
 private:
     void _parse_command_line(int argc, char * const argv[]);
@@ -88,8 +87,6 @@ private:
     static SITL *_sitl;
     static const uint16_t _rcout_port = 5502;
     static const uint16_t _simin_port = 5501;
-
-    static uint16_t _pwm_output[11];
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

libraries/AP_HAL_AVR_SITL/sitl_rc.h

@@ -1,52 +0,0 @@
-/*
-  RC input emulation
-  Code by Andrew Tridgell November 2011
- */
-
-#ifndef _SITL_RC_H
-#define _SITL_RC_H
-
-struct RC_ICR4 {
-	uint16_t channels[9]; // 9th channel is sync
-	uint32_t value;
-	uint8_t idx;
-
-	RC_ICR4() {
-		// constructor
-		channels[0] = channels[1] = channels[3] = 1500;
-		channels[4] = channels[7] = 1800;
-		channels[2] = channels[5] = channels[6] = 1000;
-		channels[8] = 4500; // sync
-		idx = 0;
-	}
-
-	/*
-	  read from ICR4 fetches next channel
-	 */
-	operator int() {
-		value += channels[idx++]*2;
-		if (idx == 9) {
-			idx = 0;
-		}
-		value = value % 40000;
-		return (uint16_t)value;
-	}
-
-
-	/*
-	   ignore rate assignment for now (needed for apm2
-	   emulation)
-	*/
-	RC_ICR4& operator=(uint16_t rate) {
-		return *this;
-	}
-
-	/*
-	  interface to set a channel value from SITL
-	 */
-	void set(uint8_t i, uint16_t v) {
-		channels[i] = v;
-	}
-};
-
-#endif