Arduino Code

/*homing() * Will connect an EE-SX671 use to set deg 0 */ void homing() { int trig = 0; for(;;) { digitalWrite(stepps, HIGH); delayMicroseconds(100); digitalWrite(stepps, LOW); delayMicroseconds(100); //sending a "Step" plus trig = digitalRead(homingP); //EE-SX671 if(trig != 0) { break; } } }

/*heartbeat() * Mavlink need a specific pack stream to let the system know that the link was still up. * It need two heartbeat pack in two second in order to stay "alive". ;) */ void heartbeat() { mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); Serial.write(buf, len); }

/*RC() * RC override is the way we control the aircraft. * 1000 = 0% * 2000 = 100% */ void RC(uint16_t ch1) { mavlink_msg_rc_channels_override_pack(sysid,compid, &msg, target_sys,target_com,ch1,0,0,0,0,0,0,0); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); Serial.write(buf, len); }

/*getglobalDeg() * not in use for now. */ /*float getglobalDeg(float nowdir) { float heading=0; if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { if(msg.msgid==MAVLINK_MSG_ID_VFR_HUD) { heading=mavlink_msg_vfr_hud_get_heading(&msg); } else{} } else{} if(nowdir>=180) { heading= -1*((nowdir+heading)%360); } if(nowdir<180) { heading= ((nowdir+heading)%360); } return heading; } */

/*TURN() * Useing mav_cmd_condition_yaw to let aircraft turn relatively */ void TURN(float deg)

{ mavlink_msg_command_long_pack(sysid,compid, &msg, target_sys,target_com,115,0,deg,0,1,1,0,0,0); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); Serial.write(buf, len); } void setup() { Serial.begin(57600); // Start serial communications pinMode(stepps, OUTPUT); // Set Step pin as OUTPUT pinMode(dir, OUTPUT); // pinMode(en, OUTPUT); // pinMode(safety, INPUT); pinMode(homingP, INPUT); // digitalWrite(dir, HIGH); // set motor ditrction to CW //!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!! //IF the direction is changed, we must change the direction of mav_cmd_condition_yaw as well, or the result will be wrong.

digitalWrite(en, LOW); // enableing the motor delay(100); }

//As the main loop in C. void loop() { homing(); //well...HOMING~!! delay(2000); for(;;) { if(pulseIn(safety, HIGH)>1500) //for safety { heartbeat(); //if safety swith is of } int volmax = 0; int vnow =0; float maxdir = 0; for (int i;i<6400;i++)//1 rotation is 32*200=6400steps { //c = Serial.read(); //not in use for now vnow =analogRead(A0); if(volmax<vnow) { volmax=vnow; maxdir = (i*360/6400); ///////trun the Steps back to deg } digitalWrite(stepps, HIGH); // turn the LED on (HIGH is the voltage level) delayMicroseconds(100); // wait for a second digitalWrite(stepps, LOW); // turn the LED off by making the voltage LOW delayMicroseconds(100); } if (maxdir<=5||maxdir>=355) // set 355~5 deg as dead zone { RC(1750); //if the aircraft is heading the place where we want. It will fly forward. } else { RC(1500); //if the aircraft isn't heading the place where we want. It will stop flying forward and start to turn. TURN(maxdir); //turn~ } } }