Merge branch 'master' of https://github.com/vjaunet/QUADCOPTER_V2

README.md

@@ -19,7 +19,7 @@ sufficient abilites for the job.
 
 The Raspberry PI hosts:
 - PID controller
-- Web interface (apache) to access system status and Camera (work in progress)
+- Web interface to access the Camera (work in progress)
 - Communication with the MPU6050 through I2C.
 - Communication to Arduino Micro through SPI
 
@@ -46,9 +46,9 @@ Usage
 ------
 
 When powered, the Quad is in a locked status so that the ESCs do not turn the
-motor on. To unlock, simply put the RC sticks to the lower rigth angle. It can
-be locked again by doinng the same operation.
-This is handled by the Arduino for more safety.
+motor on. To unlock, simply put the RC sticks to the lower center angle. It can
+be locked again by doing the same operation. This is handled by the Arduino 
+for more safety.
 
 
 Hardware
@@ -62,21 +62,25 @@ This projects includes :
 - 1 QuadCopter frame
 - 1 4 channels RC Remote + Receiver
 - 1 arduino micro
+- 1 logic level converter
 
 Wiring
 ------
 
-MPU6050 :
+MPU6050 -> RPI :
 -VDD -> 3.3V
 -GND -> GND
 -SDA -> SDA
 -SCL -> SCL
 -VIO -> 3.3V
 
-Arduino to Rpi through I2C:
--VI  -> 5V
--GND -> GND
--SPI
+Arduino -> RPI :
+-VCC  -> 5V from external Regulator
+-GND  -> GND
+-MISO -> Logic Level converter ->MISO
+-MOSI -> Logic Level converter ->MOSI
+-SCK  -> Logic Level converter ->SCK
 
 ESCs and RC Receiver on Arduino:
--....
+-Using PinChange interrupts
+-Check in the arduino code

RPI/PILOT/launch_pilot.sh

@@ -1,71 +1,118 @@
 #!/bin/bash
 
+#Led and button control GPIO pins
+LED=0
+button=2
+
 
 # trap ctrl-c and call ctrl_c()
 trap ctrl_c INT
 
 function setup(){
-    gpio mode $1 out
-    gpio mode $2 in
+    /usr/local/bin/gpio mode $1 out
+    /usr/local/bin/gpio mode $2 in
 }
 
 function set_off(){
-    gpio write $1 0
+    /usr/local/bin/gpio write $1 0
 }
 
 function set_on(){
-    gpio write $1 1
+    /usr/local/bin/gpio write $1 1
+}
+
+
+function Blink_fast()
+{
+   for i in {1..20}; do
+	set_off $1
+	sleep 0.05
+	set_on $1
+	sleep 0.05
+    done
+}
+
+function Blink_slow()
+{
+   for i in {1..3}; do
+	set_off $1
+	sleep 0.5
+	set_on $1
+	sleep 0.5
+    done
 }
 
-waitButton ()
+function waitButton ()
 {
+
+    Blink_fast $LED;
+    echo ""
  echo "Waiting for button ..."
- while [ `gpio read $1` = 1 ]; do
+ while [ `/usr/local/bin/gpio read $1` = 0 ]; do
    sleep 0.1
  done
- echo "Button pressed : Relaunching"
 
- stop_pilot
+ echo "Button pressed : stay pressed for shutdown"
+ n=0
+ while [ `/usr/local/bin/gpio read $1` = 1 ]; do
+   sleep 0.1
+   n=`expr $n + 1`
+
+ if [ "$n" -ge "10" ]; then
+     echo "Bye Bye !"
+     stop_pilot
+     Blink_slow $LED
+     sudo halt
+     exit 0
+ fi
+ done
 
+ echo "Restarting Pilot"
+ stop_pilot
  sleep 0.5
+ start_pilot;
+
+ #recursive call
+ waitButton $1;
 
- ./launch_pilot.sh;
 }
 
 function stop_pilot() {
         sudo pkill quad_pilot;
-	set_off 15;
-}
-
-
-function ctrl_c() {
-        echo "*** Trapped CTRL-C ***"
-	sudo pkill quad_pilot;
-	set_off 15;
-	exit 0
+	set_off $LED;
 }
 
-#launching quad_pilot
+function start_pilot() {
 rate_kp=1.4
 rate_ki=0.01
 rate_kd=0.02
 
-stab_kp=3.0
+stab_kp=2.5
 stab_ki=0.01
 stab_kd=0.5
 
-yaw_rate=2.5
-( sudo ./quad_pilot $rate_kp $rate_ki $rate_kd\
-       $stab_kp $stab_ki $stab_kd $yaw_rate ) &
+yaw_rate=5.0
+( sudo /home/pi/QUADCOPTER_V2/RPI/PILOT/quad_pilot $rate_kp $rate_ki $rate_kd\
+       $stab_kp $stab_ki $stab_kd $yaw_rate > quad_pilot.log 2>&1 ) &
+}
 
 
-#Led and button control
-LED=15
-button=16
+function ctrl_c() {
+        echo "*** Trapped CTRL-C ***"
+	stop_pilot;
+	exit 0
+}
+
+#MAIN --------------------------
 
+#setup LED and button to niputs
 setup $LED $button
-set_on $LED
 
+#launching quad_pilot
+start_pilot
+
+#start the button pressed watching
 waitButton $button
 
+
 wait

RPI/PILOT/main.cpp

@@ -58,12 +58,12 @@ void Set_default_PID_config(){
 }
 
 void Blink_led(){
-  // use gpio to blink an led on pin 15
-  for(int i=0;i<10;i++){
-  system("gpio write 15 0");
-  usleep(200000);
-  system("gpio write 15 1");
-  usleep(200000);
+  // use gpio to blink an led on pin 0
+  for(int i=0;i<30;i++){
+  system("/usr/local/bin/gpio write 0 0");
+  usleep(50000);
+  system("/usr/local/bin/gpio write 0 1");
+  usleep(50000);
   }
 }
 

RPI/PILOT/timer.cpp

@@ -23,14 +23,14 @@ using namespace std;
 #define N_RC_CHAN 4
 #define N_SERVO 4
 
-#define K_YAW 60
-#define K_PITCH 40
-#define K_ROLL 40
+#define K_YAW 260
+#define K_PITCH 120
+#define K_ROLL 120
 
-#define RC_MIN 1050
-#define RC_MAX 1900
+#define RC_MIN 1000
+#define RC_MAX 2000
 #define THR_MIN 890
-#define THR_MAX 1950
+#define THR_MAX 1900
 
 #define YAW 0
 #define PITCH 1
@@ -165,15 +165,30 @@ void TimerClass::sig_handler_(int signum)
     recv_checksum = ArduSPI.rwByte('S');
   }
 
+  //Bounding RC values to avoid division by zeros fex.
+    for (int i=1;i<4;i++){
+      if ( RCinput[i] > RC_MAX)	RCinput[i] = RC_MAX;
+      if ( RCinput[i] < RC_MIN)	RCinput[i] = RC_MIN;
+    }
+
+  //outputing values to logfile
+  logfile << RCinput[0] << " " << RCinput[1] << " "
+  	  << RCinput[2] << " " << RCinput[3] << " ";
+
+
   // //convert into PID usable values
   RCinput[0] = (RCinput[0] - THR_MIN)/(THR_MAX-THR_MIN) * 100.0;
-  RCinput[1] = (RCinput[1] -(RC_MAX+RC_MIN)/2.) /
+  RCinput[1] = -(RCinput[1] -(RC_MAX+RC_MIN)/2.) /
     (RC_MAX-RC_MIN) * K_YAW;
   RCinput[2] = (RCinput[2] -(RC_MAX+RC_MIN)/2.)/
     (RC_MAX-RC_MIN) * K_PITCH;
   RCinput[3] = (RCinput[3] -(RC_MAX+RC_MIN)/2.)/
     (RC_MAX-RC_MIN) * K_ROLL;
 
+  //outputing values to logfile
+  logfile << RCinput[0] << " " << RCinput[1] << " "
+  	  << RCinput[2] << " " << RCinput[3] << " ";
+
 
   #ifdef XMODE
       //Switch to Xmode instead of +mode
@@ -187,11 +202,6 @@ void TimerClass::sig_handler_(int signum)
 
   #endif
 
-
-  //outputing values to logfile
-  logfile << RCinput[0] << " " << RCinput[1] << " "
-  	  << RCinput[2] << " " << RCinput[3] << " ";
-
   // printf("Received : %6.3f %6.3f %6.3f %6.3f\n", RCinput[0],
   // 	 RCinput[1], RCinput[2], RCinput[3]);
 
@@ -200,6 +210,11 @@ void TimerClass::sig_handler_(int signum)
   while (imu.getAttitude() < 0){
   };
 
+
+  //Compensate lost of Thrust due to angle of drone
+  RCinput[0] = RCinput[0]/cos(imu.ypr[ROLL]/180*M_PI)
+    /cos(imu.ypr[PITCH]/180*M_PI);
+
   //output to logfile
   logfile << imu.ypr[YAW] << " " << imu.ypr[PITCH] << " "
 	  << imu.ypr[ROLL] << " "
@@ -287,14 +302,25 @@ void TimerClass::sig_handler_(int signum)
   //------------------------------------------------------
   //5- Send ESC update via SPI
   //compute each new ESC value
-  ESC[1] = (uint16_t)(RCinput[0]*10+1000
-  		      + PIDout[ROLL] + PIDout[YAW]);
-  ESC[3] = (uint16_t)(RCinput[0]*10+1000
-  		      - PIDout[ROLL] + PIDout[YAW]);
-  ESC[0] = (uint16_t)(RCinput[0]*10+1000
-  		      + PIDout[PITCH] - PIDout[YAW]);
-  ESC[2] = (uint16_t)(RCinput[0]*10+1000
-  		      - PIDout[PITCH] - PIDout[YAW]);
+
+  //if THR is low disable PID and be sure that ESC receive Zero
+  printf("%f \n",RCinput[0]);
+
+  if (RCinput[0] < 7.0) {
+    for (int i=0;i<4;i++){
+      ESC[i] = (uint16_t)0;
+    }
+  } else {
+
+    ESC[1] = (uint16_t)(RCinput[0]*10+1000
+			+ PIDout[ROLL] + PIDout[YAW]);
+    ESC[3] = (uint16_t)(RCinput[0]*10+1000
+			- PIDout[ROLL] + PIDout[YAW]);
+    ESC[0] = (uint16_t)(RCinput[0]*10+1000
+			+ PIDout[PITCH] - PIDout[YAW]);
+    ESC[2] = (uint16_t)(RCinput[0]*10+1000
+			- PIDout[PITCH] - PIDout[YAW]);
+  }
 
   checksum = 0;
   for (int iesc=0;iesc < N_SERVO; iesc++) {