Initial escape library

Not working yet

examples/AccelGyroMag/AccelGyroMag.cpp

@@ -0,0 +1,41 @@
+/*
+Provided to you by Emlid Ltd (c) 2014.
+twitter.com/emlidtech || www.emlid.com || info@emlid.com
+
+Example: Read accelerometer, gyroscope and magnetometer values from
+MPU9250 inertial measurement unit over SPI on Raspberry Pi + Navio.
+
+Navio's onboard MPU9250 is connected to the SPI bus on Raspberry Pi
+and can be read through /dev/spidev0.1
+
+To run this example navigate to the directory containing it and run following commands:
+make
+./AccelGyroMag
+*/
+
+#include "darkwater/MPU9250.h"
+#include "darkwater/Util.h"
+#include <stdlib.h>
+//=============================================================================
+
+int main()
+{
+    if (check_apm()) {
+        return 1;
+    }
+	//-------------------------------------------------------------------------
+	MPU9250 imu;
+	imu.initialize();
+
+	float ax, ay, az, gx, gy, gz, mx, my, mz;
+
+    //-------------------------------------------------------------------------
+
+    while(1) {
+        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+	printf("Acc: %+7.3f %+7.3f %+7.3f  ", ax, ay, az);
+	printf("Gyr: %+8.3f %+8.3f %+8.3f  ", gx, gy, gz);
+	printf("Mag: %+7.3f %+7.3f %+7.3f\n", mx, my, mz);
+	
+	usleep(500000);
+    }}

examples/AccelGyroMag/Makefile

@@ -0,0 +1,9 @@
+CC = g++
+DW = ../../darkwater
+INCLUDES = -I ../..
+
+all:
+	$(CC) $(INCLUDES) AccelGyroMag.cpp $(DW)/MPU9250.cpp $(DW)/Util.cpp -o AccelGyroMag
+
+clean:
+	rm AccelGyroMag

examples/Makefile

@@ -0,0 +1,15 @@
+MODULES = AccelGyroMag ADC AHRS Barometer FRAM GPS LED Multithread PPM-decoder Servo
+
+all:
+	for dir in $(MODULES); do \
+	cd $$dir; \
+	($(MAKE) ); \
+	cd ..; \
+	done
+	
+clean:
+	for dir in $(MODULES); do \
+	cd $$dir; \
+	($(MAKE) clean); \
+	cd ..; \
+	done

examples/Motor/Makefile

@@ -0,0 +1,9 @@
+CC = g++
+DW = ../../darkwater
+INCLUDES = -I ../..
+
+all:
+	$(CC) $(INCLUDES) Motor.cpp $(DW)/DW640.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Motor
+
+clean:
+	rm Motor

examples/Motor/Motor.cpp

@@ -0,0 +1,110 @@
+/*
+Provided to you by Emlid Ltd (c) 2014.
+twitter.com/emlidtech || www.emlid.com || info@emlid.com
+
+Example: Read accelerometer, gyroscope and magnetometer values from
+MPU9250 inertial measurement unit over SPI on Raspberry Pi + Navio.
+
+Navio's onboard MPU9250 is connected to the SPI bus on Raspberry Pi
+and can be read through /dev/spidev0.1
+
+To run this example navigate to the directory containing it and run following commands:
+make
+./AccelGyroMag
+*/
+
+#include "darkwater/DW640.h"
+#include "darkwater/Util.h"
+#include <stdlib.h>
+//=============================================================================
+
+int main()
+{
+    if (check_apm()) {
+        return 1;
+    }
+	//-------------------------------------------------------------------------
+	DW640 dw;
+	dw.initialize();
+
+	DW_Motor *dw1 = dw.getMotor(1);
+	DW_Motor *dw2 = dw.getMotor(2);
+	DW_Motor *dw3 = dw.getMotor(3);
+	DW_Motor *dw4 = dw.getMotor(4);
+	DW_Motor *dw5 = dw.getMotor(5);
+	DW_Motor *dw6 = dw.getMotor(6);
+
+	dw1->off();
+	dw2->off();
+	dw3->off();
+	dw4->off();
+	dw5->off();
+	dw6->off();
+	usleep(1000000);
+
+	printf("Set forward - \n");
+	printf("Motor 1\n");
+	dw1->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw2->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw3->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw4->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw5->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw6->setMotorSpeed( 255 );
+	usleep(1000000);
+	printf("Stopping - \n");
+	printf("Motor 1\n");
+	dw1->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw2->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw3->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw4->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw5->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw6->setMotorSpeed( 0 );
+	usleep(1000000);
+	printf("Set reverse - \n");
+	printf("Motor 1\n");
+	dw1->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("Motor 2\n");
+	dw2->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("Motor 3\n");
+	dw3->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("Motor 4\n");
+	dw4->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("Motor 5\n");
+	dw5->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("Motor 6\n");
+	dw6->setMotorSpeed( -255 );
+	usleep(1000000);
+	printf("All off \n");
+	dw1->off();
+	dw2->off();
+	dw3->off();
+	dw4->off();
+	dw5->off();
+	dw6->off();
+
+}

examples/Multithread/Makefile

@@ -0,0 +1,9 @@
+CC = g++
+NAVIO = ../../Navio
+INCLUDES = -I ../..
+
+all:
+	$(CC) $(INCLUDES) threaded_baro.cpp $(NAVIO)/MS5611.cpp $(NAVIO)/I2Cdev.cpp $(NAVIO)/Util.cpp -lpthread -o threaded_baro
+
+clean:
+	rm threaded_baro

examples/Multithread/threaded_baro.cpp

@@ -0,0 +1,66 @@
+/*
+Provided to you by Emlid Ltd (c) 2014.
+twitter.com/emlidtech || www.emlid.com || info@emlid.com
+
+Example: Get pressure from MS5611 barometer onboard of Navio shield for Raspberry Pi
+using a different thread, to update pressure info in the background
+
+To run this example navigate to the directory containing it and run following commands:
+make
+sudo ./threaded_baro
+*/
+
+#include "darkwater/MS5611.h"
+#include "darkwater/Util.h"
+#include <unistd.h>
+#include <stdio.h>
+#include <pthread.h>
+
+void * acquireBarometerData(void * barom)
+{
+    MS5611* barometer = (MS5611*)barom;
+
+    while (true) {
+        barometer->refreshPressure();
+        usleep(10000); // Waiting for pressure data ready
+        barometer->readPressure();
+
+        barometer->refreshTemperature();
+        usleep(10000); // Waiting for temperature data ready
+        barometer->readTemperature();
+
+        barometer->calculatePressureAndTemperature();
+
+        //sleep(0.5);
+    }
+
+    pthread_exit(NULL);
+}
+
+int main()
+{
+    if (check_apm()) {
+        return 1;
+    }
+
+    MS5611 baro;
+    baro.initialize();
+
+    pthread_t baro_thread;
+
+    if(pthread_create(&baro_thread, NULL, acquireBarometerData, (void *)&baro))
+    {
+        printf("Error: Failed to create barometer thread\n");
+        return 0;
+    }
+
+    while(true)
+    {
+        printf("Temperature(C): %f Pressure(millibar): %f\n", baro.getTemperature(), baro.getPressure());
+        sleep(1);
+    }
+
+    pthread_exit(NULL);
+
+    return 1;
+}

examples/PPM-decoder/Makefile

@@ -0,0 +1,14 @@
+CC = g++
+DW = ../../darkwater
+
+PIGPIO_PATH := $(PIGPIO_PATH)
+
+LIB = -L$(PIGPIO_PATH)
+INCLUDES = -I ../.. -I$(PIGPIO_PATH)
+
+
+all:
+	$(CC) $(INCLUDES) $(LIB)  PPM.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o PPM -lrt -lpthread -lpigpio
+
+clean:
+	rm PPM

examples/PPM-decoder/PPM.cpp

@@ -0,0 +1,108 @@
+/*
+This code is provided under the BSD license.
+Copyright (c) 2014, Emlid Limited. All rights reserved.
+Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com)
+twitter.com/emlidtech || www.emlid.com || info@emlid.com
+
+Application: PPM to PWM decoder for Raspberry Pi with Navio.
+
+Requres pigpio library, please install it first - http://abyz.co.uk/rpi/pigpio/
+To run this app navigate to the directory containing it and run following commands:
+make
+sudo ./PPM
+*/
+
+#include <pigpio.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <darkwater/gpio.h>
+#include "darkwater/PCA9685.h"
+#include "darkwater/Util.h"
+
+//================================ Options =====================================
+
+unsigned int samplingRate      = 1;      // 1 microsecond (can be 1,2,4,5,10)
+unsigned int ppmInputGpio      = 4;      // PPM input on Navio's 2.54 header
+unsigned int ppmSyncLength     = 4000;   // Length of PPM sync pause
+unsigned int ppmChannelsNumber = 8;      // Number of channels packed in PPM
+unsigned int servoFrequency    = 50;     // Servo control frequency
+bool verboseOutputEnabled      = true;   // Output channels values to console
+
+//============================ Objects & data ==================================
+
+PCA9685 *pwm;
+float channels[8];
+
+//============================== PPM decoder ===================================
+
+unsigned int currentChannel = 0;
+unsigned int previousTick;
+unsigned int deltaTime;
+
+void ppmOnEdge(int gpio, int level, uint32_t tick)
+{
+	if (level == 0) {	
+		deltaTime = tick - previousTick;
+		previousTick = tick;
+	
+		if (deltaTime >= ppmSyncLength) { // Sync
+			currentChannel = 0;
+
+			// RC output
+			for (int i = 0; i < ppmChannelsNumber; i++)
+			    pwm->setPWMuS(i + 3, channels[i]); // 1st Navio RC output is 3
+
+			// Console output
+			if (verboseOutputEnabled) {
+				printf("\n");
+				for (int i = 0; i < ppmChannelsNumber; i++)
+					printf("%4.f ", channels[i]);
+			}
+		}
+		else
+			if (currentChannel < ppmChannelsNumber)
+				channels[currentChannel++] = deltaTime;
+	}
+}
+
+//================================== Main ======================================
+
+using namespace Navio;
+
+int main(int argc, char *argv[])
+{
+    static const uint8_t outputEnablePin = RPI_GPIO_27;
+
+    if (check_apm()) {
+        return 1;
+    }
+    
+    Pin pin(outputEnablePin);
+
+    if (pin.init()) {
+        pin.setMode(Pin::GpioModeOutput);
+        pin.write(0); /* drive Output Enable low */
+    } else {
+        fprintf(stderr, "Output Enable not set. Are you root?");
+    }
+
+	// Servo controller setup
+
+	pwm = new PCA9685();
+	pwm->initialize();
+	pwm->setFrequency(servoFrequency);
+
+	// GPIO setup
+
+	gpioCfgClock(samplingRate, PI_DEFAULT_CLK_PERIPHERAL, 0); /* last parameter is deprecated now */
+	gpioInitialise();
+	previousTick = gpioTick();
+	gpioSetAlertFunc(ppmInputGpio, ppmOnEdge);
+
+	// Infinite sleep - all action is now happening in ppmOnEdge() function
+
+	while(1)
+		sleep(10);
+	return 0;
+}

examples/Servo/Makefile

@@ -0,0 +1,9 @@
+CC = g++
+DW = ../../darkwater
+INCLUDES = -I ../..
+
+all:
+	$(CC) $(INCLUDES) Servo.cpp $(DW)/DW640.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Servo
+
+clean:
+	rm Servo

examples/Servo/Servo.cpp

@@ -0,0 +1,42 @@
+#define SERVO_MIN 1.250 /*mS*/
+#define SERVO_MAX 1.750 /*mS*/
+
+#include "darkwater/DW640.h"
+#include "darkwater/Util.h"
+#include <stdlib.h>
+
+using namespace DarkWater;
+
+int main()
+{
+    
+    if (check_apm()) {
+        return 1;
+    }
+
+    DW640 dw;
+    dw.initialize();
+    dw.setFrequency(50);
+
+    DW_Servo *s1 = dw.getServo(1);
+    DW_Servo *s2 = dw.getServo(2);
+
+    s1->off();
+    s2->off();
+
+    printf("Start servo moves\n");
+    for( int a = 10; a >= 0; a-- ) {
+        printf("Step %d\n", a);
+        s1->setPWMmS(SERVO_MIN);
+        s2->setPWMmS(SERVO_MIN);
+        usleep(1000000);
+        s1->setPWMmS(SERVO_MAX);
+        s2->setPWMmS(SERVO_MAX);
+        usleep(1000000);
+    }
+
+    s1->off();
+    s2->off();
+
+    return 0;
+}