GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
×
1
Story
It's not only a quadcopter... it's an open-source machine!
Many of you face a problem when it comes to Flight Controller which is the brain of the Multirotor. There are plenty of ready-made pre-flashed Flight controllers in the market for cheap, But have you thought of building your own Flight controller with your Arduino? So this is the right place to understand and build your own Flight controller for your Quadcopter or Multirotor with your Arduino.
Now the questions come, where and how do I get the code for the quadcopter? So the answer is Multiwii.
MultiWii is a very popular flight controller software for DIY multi-rotors with a large community. It has support for various multi-copters with advanced features such as Bluetooth control by your smartphone, OLED display, barometer, magnetometer, GPS position hold and return to home, LED strips, and many more. So let’s build our flight controller using Arduino!
Step 1: Flight Controller Designing
Here are the Schematics for the flight controller board. you can make one on your general-purpose PCB or can Order a PCB from the manufacturer as I did.
ESC Connections
D3 << ESC 1 Signal Pin
D9 << ESC 3 Signal Pin
D10 << ESC 2 Signal Pin
D11 << ESC 4 Signal Pin
Bluetooth Module Connections
TX << RX
RX << TX
MPU-6050 Connections
A4 << SDA
A5 << SCL
LED Indicator
D8 << Anode Leg of LED
Receiver Connections
D2 << Throttle
D4 << Elerons
D5 << Ailerons
D6 << Rudder
D7 << AUX 1
Step 2: Building a Frame
I bought a DJI 450 frame and attached my motors and everything on it. You can see the video on how I did it.
Watch this video full to understand whole concept
Step 3: Attaching the Flight Controller To the Frame
Then finally attach the ESC and receiver onto the board as shown in the schematics and everything is done!
#include"Arduino.h"#include"config.h"#include"def.h"#include"types.h"#include"GPS.h"#include"Serial.h"#include"Sensors.h"#include"MultiWii.h"#include"EEPROM.h"#include<math.h>#if GPS//Function prototypes for other GPS functions//These perhaps could go to the gps.h file, however these are local to the gps.cpp staticvoidGPS_bearing(int32_t*lat1,int32_t*lon1,int32_t*lat2,int32_t*lon2,int32_t*bearing);staticvoidGPS_distance_cm(int32_t*lat1,int32_t*lon1,int32_t*lat2,int32_t*lon2,uint32_t*dist);staticvoidGPS_calc_velocity(void);staticvoidGPS_calc_location_error(int32_t*target_lat,int32_t*target_lng,int32_t*gps_lat,int32_t*gps_lng);staticvoidGPS_calc_poshold(void);staticuint16_tGPS_calc_desired_speed(uint16_tmax_speed,bool_slow);staticvoidGPS_calc_nav_rate(uint16_tmax_speed);int32_twrap_18000(int32_tang);staticboolcheck_missed_wp(void);voidGPS_calc_longitude_scaling(int32_tlat);staticvoidGPS_update_crosstrack(void);int32_twrap_36000(int32_tang);// Leadig filter - TODO: rewrite to normal C instead of C++// Set up gps lag#if defined(UBLOX) || defined (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS has a smaller lag than MTK and other#else#define GPS_LAG 1.0f //We assumes that MTK GPS has a 1 sec lag#endif staticint32_tGPS_coord_lead[2];// Lead filtered gps coordinatesclassLeadFilter{public:LeadFilter():_last_velocity(0){}// setup min and max radio values in CLIint32_tget_position(int32_tpos,int16_tvel,floatlag_in_seconds=1.0);voidclear(){_last_velocity=0;}private:int16_t_last_velocity;};int32_tLeadFilter::get_position(int32_tpos,int16_tvel,floatlag_in_seconds){int16_taccel_contribution=(vel-_last_velocity)*lag_in_seconds*lag_in_seconds;int16_tvel_contribution=vel*lag_in_seconds;// store velocity for next iteration_last_velocity=vel;returnpos+vel_contribution+accel_contribution;}LeadFilterxLeadFilter;// Long GPS lag filter LeadFilteryLeadFilter;// Lat GPS lag filter typedefstructPID_PARAM_{floatkP;floatkI;floatkD;floatImax;}PID_PARAM;PID_PARAMposholdPID_PARAM;PID_PARAMposhold_ratePID_PARAM;PID_PARAMnavPID_PARAM;typedefstructPID_{floatintegrator;// integrator valueint32_tlast_input;// last input for derivativefloatlastderivative;// last derivative for low-pass filterfloatoutput;floatderivative;}PID;PIDposholdPID[2];PIDposhold_ratePID[2];PIDnavPID[2];int32_tget_P(int32_terror,structPID_PARAM_*pid){return(float)error*pid->kP;}int32_tget_I(int32_terror,float*dt,structPID_*pid,structPID_PARAM_*pid_param){pid->integrator+=((float)error*pid_param->kI)**dt;pid->integrator=constrain(pid->integrator,-pid_param->Imax,pid_param->Imax);returnpid->integrator;}int32_tget_D(int32_tinput,float*dt,structPID_*pid,structPID_PARAM_*pid_param){// dt in millisecondspid->derivative=(input-pid->last_input)/*dt;/// Low pass filter cut frequency for derivative calculation.floatfilter=7.9577e-3;// Set to "1 / ( 2 * PI * f_cut )";// Examples for _filter:// f_cut = 10 Hz -> _filter = 15.9155e-3// f_cut = 15 Hz -> _filter = 10.6103e-3// f_cut = 20 Hz -> _filter = 7.9577e-3// f_cut = 25 Hz -> _filter = 6.3662e-3// f_cut = 30 Hz -> _filter = 5.3052e-3// discrete low pass filter, cuts out the// high frequency noise that can drive the controller crazypid->derivative=pid->lastderivative+(*dt/(filter+*dt))*(pid->derivative-pid->lastderivative);// update statepid->last_input=input;pid->lastderivative=pid->derivative;// add in derivative componentreturnpid_param->kD*pid->derivative;}voidreset_PID(structPID_*pid){pid->integrator=0;pid->last_input=0;pid->lastderivative=0;}#define _X 1#define _Y 0#define RADX100 0.000174532925 uint8_tland_detect;//Detect land (extern)staticuint32_tland_settle_timer;uint8_tGPS_Frame;// a valid GPS_Frame was detected, and data is ready for nav computationstaticfloatdTnav;// Delta Time in milliseconds for navigation computations, updated with every good GPS readstaticint16_tactual_speed[2]={0,0};staticfloatGPS_scaleLonDown;// this is used to offset the shrinking longitude as we go towards the poles// The difference between the desired rate of travel and the actual rate of travel// updated after GPS read - 5-10hzstaticint16_trate_error[2];staticint32_terror[2];staticint32_tGPS_WP[2];//Currently used WPstaticint32_tGPS_FROM[2];//the pervious waypoint for precise track followingint32_ttarget_bearing;// This is the angle from the copter to the "next_WP" location in degrees * 100staticint32_toriginal_target_bearing;// deg * 100, The original angle to the next_WP when the next_WP was set, Also used to check when we pass a WPstaticint16_tcrosstrack_error;// The amount of angle correction applied to target_bearing to bring the copter back on its optimum pathuint32_twp_distance;// distance between plane and next_WP in cmstaticuint16_twaypoint_speed_gov;// used for slow speed wind up when start navigation;////////////////////////////////////////////////////////////////////////////////////// moving average filter variables//#define GPS_FILTER_VECTOR_LENGTH 5staticuint8_tGPS_filter_index=0;staticint32_tGPS_filter[2][GPS_FILTER_VECTOR_LENGTH];staticint32_tGPS_filter_sum[2];staticint32_tGPS_read[2];staticint32_tGPS_filtered[2];staticint32_tGPS_degree[2];//the lat lon degree without any decimals (lat/10 000 000)staticuint16_tfraction3[2];staticint16_tnav_takeoff_bearing;// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home//Main navigation processor and state engine// TODO: add proceesing states to ease processing burden uint8_tGPS_Compute(void){unsignedcharaxis;uint32_tdist;//temp variable to store dist to copterint32_tdir;//temp variable to store dir to copterstaticuint32_tnav_loopTimer;//check that we have a valid frame, if not then return immediatlyif(GPS_Frame==0)return0;elseGPS_Frame=0;//check home position and set it if it was not setif(f.GPS_FIX&&GPS_numSat>=5){#if !defined(DONT_RESET_HOME_AT_ARM)if(!f.ARMED){f.GPS_FIX_HOME=0;}#endifif(!f.GPS_FIX_HOME&&f.ARMED){GPS_reset_home_position();}//Apply moving average filter to GPS dataif(GPS_conf.filtering){GPS_filter_index=(GPS_filter_index+1)%GPS_FILTER_VECTOR_LENGTH;for(axis=0;axis<2;axis++){GPS_read[axis]=GPS_coord[axis];//latest unfiltered data is in GPS_latitude and GPS_longitudeGPS_degree[axis]=GPS_read[axis]/10000000;// get the degree to assure the sum fits to the int32_t// How close we are to a degree line ? its the first three digits from the fractions of degree// later we use it to Check if we are close to a degree line, if yes, disable averaging,fraction3[axis]=(GPS_read[axis]-GPS_degree[axis]*10000000)/10000;GPS_filter_sum[axis]-=GPS_filter[axis][GPS_filter_index];GPS_filter[axis][GPS_filter_index]=GPS_read[axis]-(GPS_degree[axis]*10000000);GPS_filter_sum[axis]+=GPS_filter[axis][GPS_filter_index];GPS_filtered[axis]=GPS_filter_sum[axis]/GPS_FILTER_VECTOR_LENGTH+(GPS_degree[axis]*10000000);if(NAV_state==NAV_STATE_HOLD_INFINIT||NAV_state==NAV_STATE_HOLD_TIMED){//we use gps averaging only in poshold mode...if(fraction3[axis]>1&&fraction3[axis]<999)GPS_coord[axis]=GPS_filtered[axis];}}}//dTnav calculation//Time for calculating x,y speed and navigation pidsdTnav=(float)(millis()-nav_loopTimer)/1000.0;nav_loopTimer=millis();// prevent runup from bad GPSdTnav=min(dTnav,1.0);//calculate distance and bearings for gui and other stuff continously - From home to copterGPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dir);GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist);GPS_distanceToHome=dist/100;GPS_directionToHome=dir/100;if(!f.GPS_FIX_HOME){//If we don't have home set, do not display anythingGPS_distanceToHome=0;GPS_directionToHome=0;}//Check fence setting and execute RTH if neccessary//TODO: autolandingif((GPS_conf.fence>0)&&(GPS_conf.fence<GPS_distanceToHome)&&(f.GPS_mode!=GPS_MODE_RTH)){init_RTH();}//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigatingGPS_calc_velocity();//Navigation state engineif(f.GPS_mode!=GPS_MODE_NONE){//ok we are navigating ###0002 //do gps nav calculations here, these are common for nav and poshold GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_bearing);if(GPS_conf.lead_filter){GPS_distance_cm(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);}else{GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);}// Adjust altitude // if we are holding position and reached target altitude, then ignore altitude nav, and let the user trim altif(!((NAV_state==NAV_STATE_HOLD_INFINIT)&&(alt_change_flag==REACHED_ALT))){if(!f.LAND_IN_PROGRESS){alt_to_hold=get_new_altitude();AltHold=alt_to_hold;}}int16_tspeed=0;//Desired navigation speedswitch(NAV_state)//Navigation state machine{caseNAV_STATE_NONE://Just for clarity, do nothing when nav_state is nonebreak;caseNAV_STATE_LAND_START:GPS_calc_poshold();//Land in position holdland_settle_timer=millis();NAV_state=NAV_STATE_LAND_SETTLE;break;caseNAV_STATE_LAND_SETTLE:GPS_calc_poshold();if(millis()-land_settle_timer>5000)NAV_state=NAV_STATE_LAND_START_DESCENT;break;caseNAV_STATE_LAND_START_DESCENT:GPS_calc_poshold();//Land in position holdf.THROTTLE_IGNORED=1;//Ignore Throtte stick inputf.GPS_BARO_MODE=1;//Take control of BARO modeland_detect=0;//Reset land detectorf.LAND_COMPLETED=0;f.LAND_IN_PROGRESS=1;// Flag land processNAV_state=NAV_STATE_LAND_IN_PROGRESS;break;caseNAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold();check_land();//Call land detectorif(f.LAND_COMPLETED){nav_timer_stop=millis()+5000;NAV_state=NAV_STATE_LANDED;}break;caseNAV_STATE_LANDED:// Disarm if THROTTLE stick is at minimum or 5sec past after land detectedif(rcData[THROTTLE]<MINCHECK||nav_timer_stop<=millis()){//Throttle at minimum or 5sec passed.go_disarm();f.OK_TO_ARM=0;//Prevent rearmingNAV_state=NAV_STATE_NONE;//Disable position holding.... prevent flippoverf.GPS_BARO_MODE=0;f.LAND_COMPLETED=0;f.LAND_IN_PROGRESS=0;land_detect=0;f.THROTTLE_IGNORED=0;GPS_reset_nav();}break;caseNAV_STATE_HOLD_INFINIT://Constant position hold, no timer. Only an rcOption change can exit from thisGPS_calc_poshold();break;caseNAV_STATE_HOLD_TIMED:if(nav_timer_stop==0){//We are start a timed posholdnav_timer_stop=millis()+1000*nav_hold_time;//Set when we will continue}elseif(nav_timer_stop<=millis()){//did we reach our time limit ?if(mission_step.flag!=MISSION_FLAG_END){NAV_state=NAV_STATE_PROCESS_NEXT;//if yes then process next mission step}NAV_error=NAV_ERROR_TIMEWAIT;}GPS_calc_poshold();//BTW hold position till next commandbreak;caseNAV_STATE_RTH_START:if((alt_change_flag==REACHED_ALT)||(!GPS_conf.wait_for_rth_alt)){//Wait until we reach RTH altitudeGPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON],&GPS_coord[LAT],&GPS_coord[LON]);//If we reached then change mode and start RTHNAV_state=NAV_STATE_RTH_ENROUTE;NAV_error=NAV_ERROR_NONE;}else{GPS_calc_poshold();//hold position till we reach RTH altNAV_error=NAV_ERROR_WAIT_FOR_RTH_ALT;}break;caseNAV_STATE_RTH_ENROUTE://Doing RTH navigationspeed=GPS_calc_desired_speed(GPS_conf.nav_speed_max,GPS_conf.slow_nav);GPS_calc_nav_rate(speed);GPS_adjust_heading();if((wp_distance<=GPS_conf.wp_radius)||check_missed_wp()){//if yes switch to poshold modeif(mission_step.parameter1==0)NAV_state=NAV_STATE_HOLD_INFINIT;elseNAV_state=NAV_STATE_LAND_START;// if parameter 1 in RTH step is non 0 then land at homeif(GPS_conf.nav_rth_takeoff_heading){magHold=nav_takeoff_bearing;}}break;caseNAV_STATE_WP_ENROUTE:speed=GPS_calc_desired_speed(GPS_conf.nav_speed_max,GPS_conf.slow_nav);GPS_calc_nav_rate(speed);GPS_adjust_heading();if((wp_distance<=GPS_conf.wp_radius)||check_missed_wp()){//This decides what happen when we reached the WP coordinates if(mission_step.action==MISSION_LAND){//AutolandNAV_state=NAV_STATE_LAND_START;//Start landingset_new_altitude(alt.EstAlt);//Stop any altitude changes}elseif(mission_step.flag==MISSION_FLAG_END){//If this was the last mission step (flag set by the mission planner), then switch to posholdNAV_state=NAV_STATE_HOLD_INFINIT;NAV_error=NAV_ERROR_FINISH;}elseif(mission_step.action==MISSION_HOLD_UNLIM){//If mission_step was POSHOLD_UNLIM and we reached the position then switch to poshold unlimitedNAV_state=NAV_STATE_HOLD_INFINIT;NAV_error=NAV_ERROR_FINISH;}elseif(mission_step.action==MISSION_HOLD_TIME){//If mission_step was a timed poshold then initiate timed posholdnav_hold_time=mission_step.parameter1;nav_timer_stop=0;//This indicates that we are starting a timed posholdNAV_state=NAV_STATE_HOLD_TIMED;}else{NAV_state=NAV_STATE_PROCESS_NEXT;//Otherwise process next step}}break;caseNAV_STATE_DO_JUMP:if(jump_times<0){//Jump unconditionally (supposed to be -1) -10 should not be herenext_step=mission_step.parameter1;NAV_state=NAV_STATE_PROCESS_NEXT;}if(jump_times==0){jump_times=-10;//reset jump counterif(mission_step.flag==MISSION_FLAG_END){//If this was the last mission step (flag set by the mission planner), then switch to posholdNAV_state=NAV_STATE_HOLD_INFINIT;NAV_error=NAV_ERROR_FINISH;}elseNAV_state=NAV_STATE_PROCESS_NEXT;}if(jump_times>0){//if zero not reached do a jumpnext_step=mission_step.parameter1;NAV_state=NAV_STATE_PROCESS_NEXT;jump_times--;}break;caseNAV_STATE_PROCESS_NEXT://Processing next mission stepNAV_error=NAV_ERROR_NONE;if(!recallWP(next_step)){abort_mission(NAV_ERROR_WP_CRC);}else{switch(mission_step.action){//Waypoiny and hold commands all starts with an enroute status it includes the LAND command toocaseMISSION_WAYPOINT:caseMISSION_HOLD_TIME:caseMISSION_HOLD_UNLIM:caseMISSION_LAND:set_new_altitude(mission_step.altitude);GPS_set_next_wp(&mission_step.pos[LAT],&mission_step.pos[LON],&GPS_prev[LAT],&GPS_prev[LON]);if((wp_distance/100)>=GPS_conf.safe_wp_distance)abort_mission(NAV_ERROR_TOOFAR);elseNAV_state=NAV_STATE_WP_ENROUTE;GPS_prev[LAT]=mission_step.pos[LAT];//Save wp coordinates for precise route calcGPS_prev[LON]=mission_step.pos[LON];break;caseMISSION_RTH:f.GPS_head_set=0;if(GPS_conf.rth_altitude==0&&mission_step.altitude==0)//if config and mission_step alt is zero set_new_altitude(alt.EstAlt);// RTH returns at the actual altitude else{uint32_trth_alt;if(mission_step.altitude==0)rth_alt=GPS_conf.rth_altitude*100;//altitude in mission step has priorityelserth_alt=mission_step.altitude;if(alt.EstAlt<rth_alt)set_new_altitude(rth_alt);//BUt only if we are below it.elseset_new_altitude(alt.EstAlt);}NAV_state=NAV_STATE_RTH_START;break;caseMISSION_JUMP:if(jump_times==-10)jump_times=mission_step.parameter2;if(mission_step.parameter1>0&&mission_step.parameter1<mission_step.number)NAV_state=NAV_STATE_DO_JUMP;else//Error situation, invalid jump targetabort_mission(NAV_ERROR_INVALID_JUMP);break;caseMISSION_SET_HEADING:GPS_poi[LAT]=0;GPS_poi[LON]=0;// zeroing this out clears the possible pervious set_poiif(mission_step.parameter1<0)f.GPS_head_set=0;else{f.GPS_head_set=1;GPS_directionToPoi=mission_step.parameter1;}break;caseMISSION_SET_POI:GPS_poi[LAT]=mission_step.pos[LAT];GPS_poi[LON]=mission_step.pos[LON];f.GPS_head_set=1;break;default://if we got an unknown action code abort mission and hold positionabort_mission(NAV_ERROR_INVALID_DATA);break;}next_step++;//Prepare for the next step}break;}// switch end}//end of gps calcs ###0002 }return1;}// End of GPS_compute// Abort current mission with the given error code (switch to poshold_infinit)voidabort_mission(unsignedcharerror_code){GPS_set_next_wp(&GPS_coord[LAT],&GPS_coord[LON],&GPS_coord[LAT],&GPS_coord[LON]);NAV_error=error_code;NAV_state=NAV_STATE_HOLD_INFINIT;}//Adjusting heading according to settings - MAG mode must be enabledvoidGPS_adjust_heading(){//TODO: Add slow windup for large heading change//This controls the headingif(f.GPS_head_set){// We have seen a SET_POI or a SET_HEADING commandif(GPS_poi[LAT]==0)magHold=wrap_18000((GPS_directionToPoi*100))/100;else{GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&GPS_directionToPoi);GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&wp_distance);magHold=GPS_directionToPoi/100;}}else{// heading controlled by the standard definesif(GPS_conf.nav_controls_heading){if(GPS_conf.nav_tail_first){magHold=wrap_18000(target_bearing-18000)/100;}else{magHold=wrap_18000(target_bearing)/100;}}}}#define LAND_DETECT_THRESHOLD 40 //Counts of land situation#define BAROPIDMIN -180 //BaroPID reach this if we landed.....//Check if we landed or notvoidcheck_land(){// detect whether we have landed by watching for low climb rate and throttle controlif((abs(alt.vario)<20)&&(BaroPID<BAROPIDMIN)){if(!f.LAND_COMPLETED){if(land_detect<LAND_DETECT_THRESHOLD){land_detect++;}else{f.LAND_COMPLETED=1;land_detect=0;}}}else{// we've detected movement up or down so reset land_detectorland_detect=0;if(f.LAND_COMPLETED){f.LAND_COMPLETED=0;}}}int32_tget_altitude_error(){returnalt_to_hold-alt.EstAlt;}voidclear_new_altitude(){alt_change_flag=REACHED_ALT;}voidforce_new_altitude(int32_t_new_alt){alt_to_hold=_new_alt;target_altitude=_new_alt;alt_change_flag=REACHED_ALT;}voidset_new_altitude(int32_t_new_alt){//Limit maximum altitude commandif(_new_alt>GPS_conf.nav_max_altitude*100)_new_alt=GPS_conf.nav_max_altitude*100;if(_new_alt==alt.EstAlt){force_new_altitude(_new_alt);return;}// We start at the current location altitude and gradually change altalt_to_hold=alt.EstAlt;// for calculating the delta timealt_change_timer=millis();// save the target altitudetarget_altitude=_new_alt;// reset our altitude integratoralt_change=0;// save the original altitudeoriginal_altitude=alt.EstAlt;// to decide if we have reached the target altitudeif(target_altitude>original_altitude){// we are below, going upalt_change_flag=ASCENDING;}elseif(target_altitude<original_altitude){// we are above, going downalt_change_flag=DESCENDING;}else{// No Changealt_change_flag=REACHED_ALT;}}int32_tget_new_altitude(){// returns a new altitude which feeded into the alt.hold controllerif(alt_change_flag==ASCENDING){// we are below, going upif(alt.EstAlt>=target_altitude)alt_change_flag=REACHED_ALT;// we shouldn't command past our targetif(alt_to_hold>=target_altitude)returntarget_altitude;}elseif(alt_change_flag==DESCENDING){// we are above, going downif(alt.EstAlt<=target_altitude)alt_change_flag=REACHED_ALT;// we shouldn't command past our targetif(alt_to_hold<=target_altitude)returntarget_altitude;}// if we have reached our target altitude, return the target altif(alt_change_flag==REACHED_ALT)returntarget_altitude;int32_tdiff=abs(alt_to_hold-target_altitude);// scale is how we generate a desired rate from the elapsed time// a smaller scale means faster ratesint8_t_scale=4;if(alt_to_hold<target_altitude){// we are below the target altif(diff<200)_scale=4;else_scale=3;}else{// we are above the target, going downif(diff<400)_scale=5;//Slow down if only 4meters aboveif(diff<100)_scale=6;//Slow down further if within 1meter}// we use the elapsed time as our altitude offset// 1000 = 1 sec// 1000 >> 4 = 64cm/s descent by defaultint32_tchange=(millis()-alt_change_timer)>>_scale;if(alt_change_flag==ASCENDING){alt_change+=change;}else{alt_change-=change;}// for generating delta timealt_change_timer=millis();returnoriginal_altitude+alt_change;}//////////////////////////////////////////////////////////////////////////////////////PID based GPS navigation functions//Author : EOSBandi//Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//original constraint does not work with variablesint16_tconstrain_int16(int16_tamt,int16_tlow,int16_thigh){return((amt)<(low)?(low):((amt)>(high)?(high):(amt)));}////////////////////////////////////////////////////////////////////////////////////// this is used to offset the shrinking longitude as we go towards the poles// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter//voidGPS_calc_longitude_scaling(int32_tlat){GPS_scaleLonDown=cos(lat*1.0e-7f*0.01745329251f);}////////////////////////////////////////////////////////////////////////////////////// Sets the waypoint to navigate, reset neccessary variables and calculate initial values//voidGPS_set_next_wp(int32_t*lat_to,int32_t*lon_to,int32_t*lat_from,int32_t*lon_from){GPS_WP[LAT]=*lat_to;GPS_WP[LON]=*lon_to;GPS_FROM[LAT]=*lat_from;GPS_FROM[LON]=*lon_from;GPS_calc_longitude_scaling(*lat_to);GPS_bearing(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_bearing);GPS_distance_cm(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_FROM[LAT],&GPS_FROM[LON]);waypoint_speed_gov=GPS_conf.nav_speed_min;original_target_bearing=target_bearing;}////////////////////////////////////////////////////////////////////////////////////// Check if we missed the destination somehow//staticboolcheck_missed_wp(void){int32_ttemp;temp=target_bearing-original_target_bearing;temp=wrap_18000(temp);return(abs(temp)>10000);// we passed the waypoint by 100 degrees}////////////////////////////////////////////////////////////////////////////////////// Get distance between two points in cm// Get bearing from pos1 to pos2, returns an 1deg = 100 precisionvoidGPS_bearing(int32_t*lat1,int32_t*lon1,int32_t*lat2,int32_t*lon2,int32_t*bearing){int32_toff_x=*lon2-*lon1;int32_toff_y=(*lat2-*lat1)/GPS_scaleLonDown;*bearing=9000+atan2(-off_y,off_x)*5729.57795f;//Convert the output redians to 100xdegif(*bearing<0)*bearing+=36000;}voidGPS_distance_cm(int32_t*lat1,int32_t*lon1,int32_t*lat2,int32_t*lon2,uint32_t*dist){floatdLat=(float)(*lat2-*lat1);// difference of latitude in 1/10 000 000 degreesfloatdLon=(float)(*lon2-*lon1)*GPS_scaleLonDown;//x*dist=sqrt(sq(dLat)+sq(dLon))*1.11318845f;}//*******************************************************************************************************// calc_velocity_and_filtered_position - velocity in lon and lat directions calculated from GPS position// and accelerometer data// lon_speed expressed in cm/s. positive numbers mean moving east// lat_speed expressed in cm/s. positive numbers when moving north// Note: we use gps locations directly to calculate velocity instead of asking gps for velocity because// this is more accurate below 1.5m/s// Note: even though the positions are projected using a lead filter, the velocities are calculated// from the unaltered gps locations. We do not want noise from our lead filter affecting velocity//*******************************************************************************************************staticvoidGPS_calc_velocity(void){staticint16_tspeed_old[2]={0,0};staticint32_tlast[2]={0,0};staticuint8_tinit=0;if(init){floattmp=1.0/dTnav;actual_speed[_X]=(float)(GPS_coord[LON]-last[LON])*GPS_scaleLonDown*tmp;actual_speed[_Y]=(float)(GPS_coord[LAT]-last[LAT])*tmp;//TODO: Check unrealistic speed changes and signal navigation about posibble gps signal degradationif(!GPS_conf.lead_filter){actual_speed[_X]=(actual_speed[_X]+speed_old[_X])/2;actual_speed[_Y]=(actual_speed[_Y]+speed_old[_Y])/2;speed_old[_X]=actual_speed[_X];speed_old[_Y]=actual_speed[_Y];}}init=1;last[LON]=GPS_coord[LON];last[LAT]=GPS_coord[LAT];if(GPS_conf.lead_filter){GPS_coord_lead[LON]=xLeadFilter.get_position(GPS_coord[LON],actual_speed[_X],GPS_LAG);GPS_coord_lead[LAT]=yLeadFilter.get_position(GPS_coord[LAT],actual_speed[_Y],GPS_LAG);}}////////////////////////////////////////////////////////////////////////////////////// Calculate a location error between two gps coordinates// Because we are using lat and lon to do our distance errors here's a quick chart:// 100 = 1m// 1000 = 11m = 36 feet// 1800 = 19.80m = 60 feet// 3000 = 33m// 10000 = 111m//staticvoidGPS_calc_location_error(int32_t*target_lat,int32_t*target_lng,int32_t*gps_lat,int32_t*gps_lng){error[LON]=(float)(*target_lng-*gps_lng)*GPS_scaleLonDown;// X Errorerror[LAT]=*target_lat-*gps_lat;// Y Error}////////////////////////////////////////////////////////////////////////////////////// Calculate nav_lat and nav_lon from the x and y error and the speed//// TODO: check that the poshold target speed constraint can be increased for snappier poshold lockstaticvoidGPS_calc_poshold(void){int32_td;int32_ttarget_speed;uint8_taxis;for(axis=0;axis<2;axis++){target_speed=get_P(error[axis],&posholdPID_PARAM);// calculate desired speed from lat/lon errortarget_speed=constrain(target_speed,-100,100);// Constrain the target speed in poshold mode to 1m/s it helps avoid runaways..rate_error[axis]=target_speed-actual_speed[axis];// calc the speed errornav[axis]=get_P(rate_error[axis],&poshold_ratePID_PARAM)+get_I(rate_error[axis]+error[axis],&dTnav,&poshold_ratePID[axis],&poshold_ratePID_PARAM);d=get_D(error[axis],&dTnav,&poshold_ratePID[axis],&poshold_ratePID_PARAM);d=constrain(d,-2000,2000);// get rid of noiseif(abs(actual_speed[axis])<50)d=0;nav[axis]+=d;// nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);nav[axis]=constrain_int16(nav[axis],-GPS_conf.nav_bank_max,GPS_conf.nav_bank_max);navPID[axis].integrator=poshold_ratePID[axis].integrator;}}////////////////////////////////////////////////////////////////////////////////////// Calculate the desired nav_lat and nav_lon for distance flying such as RTH and WP//staticvoidGPS_calc_nav_rate(uint16_tmax_speed){floattrig[2];int32_ttarget_speed[2];int32_ttilt;uint8_taxis;GPS_update_crosstrack();int16_tcross_speed=crosstrack_error*(GPS_conf.crosstrack_gain/100.0);//check is it ok ?cross_speed=constrain(cross_speed,-200,200);cross_speed=-cross_speed;floattemp=(9000l-target_bearing)*RADX100;trig[_X]=cos(temp);trig[_Y]=sin(temp);target_speed[_X]=max_speed*trig[_X]-cross_speed*trig[_Y];target_speed[_Y]=cross_speed*trig[_X]+max_speed*trig[_Y];for(axis=0;axis<2;axis++){rate_error[axis]=target_speed[axis]-actual_speed[axis];rate_error[axis]=constrain(rate_error[axis],-1000,1000);nav[axis]=get_P(rate_error[axis],&navPID_PARAM)+get_I(rate_error[axis],&dTnav,&navPID[axis],&navPID_PARAM)+get_D(rate_error[axis],&dTnav,&navPID[axis],&navPID_PARAM);// nav[axis] = constrain(nav[axis],-NAV_BANK_MAX,NAV_BANK_MAX);nav[axis]=constrain_int16(nav[axis],-GPS_conf.nav_bank_max,GPS_conf.nav_bank_max);poshold_ratePID[axis].integrator=navPID[axis].integrator;}}staticvoidGPS_update_crosstrack(void){// Crosstrack Error// ----------------// If we are too far off or too close we don't do track followingfloattemp=(target_bearing-original_target_bearing)*RADX100;crosstrack_error=sin(temp)*wp_distance;// Meters we are off track line}////////////////////////////////////////////////////////////////////////////////////// Determine desired speed when navigating towards a waypoint, also implement slow // speed rampup when starting a navigation//// |< WP Radius// 0 1 2 3 4 5 6 7 8m// ...|...|...|...|...|...|...|...|// 100 | 200 300 400cm/s// | +|+// |< we should slow to 1 m/s as we hit the target//staticuint16_tGPS_calc_desired_speed(uint16_tmax_speed,bool_slow){if(_slow){max_speed=min(max_speed,wp_distance/2);}else{max_speed=min(max_speed,wp_distance);max_speed=max(max_speed,GPS_conf.nav_speed_min);// go at least nav_speed_min}// limit the ramp up of the speed// waypoint_speed_gov is reset to 0 at each new WP commandif(max_speed>waypoint_speed_gov){waypoint_speed_gov+=(int)(100.0*dTnav);// increase at .5/msmax_speed=waypoint_speed_gov;}returnmax_speed;}////////////////////////////////////////////////////////////////////////////////////// Utilities//int32_twrap_36000(int32_tang){if(ang>36000)ang-=36000;if(ang<0)ang+=36000;returnang;}/* * EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution * with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased * resolution also increased precision of nav calculations*/#define DIGIT_TO_VAL(_x) (_x - '0')uint32_tGPS_coord_to_degrees(char*s){char*p,*q;uint8_tdeg=0,min=0;unsignedintfrac_min=0;uint8_ti;// scan for decimal point or end of fieldfor(p=s;isdigit(*p);p++);q=s;// convert degreeswhile((p-q)>2){if(deg)deg*=10;deg+=DIGIT_TO_VAL(*q++);}// convert minuteswhile(p>q){if(min)min*=10;min+=DIGIT_TO_VAL(*q++);}// convert fractional minutes// expect up to four digits, result is in// ten-thousandths of a minuteif(*p=='.'){q=p+1;for(i=0;i<4;i++){frac_min*=10;if(isdigit(*q))frac_min+=*q++-'0';}}returndeg*10000000UL+(min*1000000UL+frac_min*100UL)/6;}// helper functions uint16_tgrab_fields(char*src,uint8_tmult){// convert string to uint16uint8_ti;uint16_ttmp=0;for(i=0;src[i]!=0;i++){if(src[i]=='.'){i++;if(mult==0)break;elsesrc[i+mult]=0;}tmp*=10;if(src[i]>='0'&&src[i]<='9')tmp+=src[i]-'0';}returntmp;}uint8_thex_c(uint8_tn){// convert '0'..'9','A'..'F' to 0..15n-='0';if(n>9)n-=7;n&=0x0F;returnn;}//************************************************************************// Common GPS functions //voidinit_RTH(){f.GPS_mode=GPS_MODE_RTH;// Set GPS_mode to RTHf.GPS_BARO_MODE=true;GPS_hold[LAT]=GPS_coord[LAT];//All RTH starts with a poshold GPS_hold[LON]=GPS_coord[LON];//This allows to raise to rth altitudeGPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON],&GPS_hold[LAT],&GPS_hold[LON]);NAV_paused_at=0;if(GPS_conf.rth_altitude==0)set_new_altitude(alt.EstAlt);//Return at actual altitudeelse{// RTH altitude is defined, but we use it only if we are below itif(alt.EstAlt<GPS_conf.rth_altitude*100)set_new_altitude(GPS_conf.rth_altitude*100);elseset_new_altitude(alt.EstAlt);}f.GPS_head_set=0;//Allow the RTH ti handle headingNAV_state=NAV_STATE_RTH_START;//NAV engine status is Starting RTH.}voidGPS_reset_home_position(void){if(f.GPS_FIX&&GPS_numSat>=5){GPS_home[LAT]=GPS_coord[LAT];GPS_home[LON]=GPS_coord[LON];GPS_calc_longitude_scaling(GPS_coord[LAT]);//need an initial value for distance and bearing calcnav_takeoff_bearing=att.heading;//save takeoff heading//TODO: Set ground altitudef.GPS_FIX_HOME=1;}}//reset navigation (stop the navigation processor, and clear nav)voidGPS_reset_nav(void){uint8_ti;for(i=0;i<2;i++){nav[i]=0;reset_PID(&posholdPID[i]);reset_PID(&poshold_ratePID[i]);reset_PID(&navPID[i]);NAV_state=NAV_STATE_NONE;//invalidate JUMP counterjump_times=-10;//reset next step counternext_step=1;//Clear poiGPS_poi[LAT]=0;GPS_poi[LON]=0;f.GPS_head_set=0;}}//Get the relevant P I D values and set the PID controllers voidGPS_set_pids(void){posholdPID_PARAM.kP=(float)conf.pid[PIDPOS].P8/100.0;posholdPID_PARAM.kI=(float)conf.pid[PIDPOS].I8/100.0;posholdPID_PARAM.Imax=POSHOLD_RATE_IMAX*100;poshold_ratePID_PARAM.kP=(float)conf.pid[PIDPOSR].P8/10.0;poshold_ratePID_PARAM.kI=(float)conf.pid[PIDPOSR].I8/100.0;poshold_ratePID_PARAM.kD=(float)conf.pid[PIDPOSR].D8/1000.0;poshold_ratePID_PARAM.Imax=POSHOLD_RATE_IMAX*100;navPID_PARAM.kP=(float)conf.pid[PIDNAVR].P8/10.0;navPID_PARAM.kI=(float)conf.pid[PIDNAVR].I8/100.0;navPID_PARAM.kD=(float)conf.pid[PIDNAVR].D8/1000.0;navPID_PARAM.Imax=POSHOLD_RATE_IMAX*100;}//It was moved here since even i2cgps code needs itint32_twrap_18000(int32_tang){if(ang>18000)ang-=36000;if(ang<-18000)ang+=36000;returnang;}/**************************************************************************************//**************************************************************************************/...Thisfilehasbeentruncated,pleasedownloadittoseeitsfullcontents.
Comments