Created
August 10, 2021 10:42
-
-
Save jimitjaishwal/09cca366c5d1f47b172d06ef60fa4de1 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // note that I only put here some of only gps releted code importent | |
| // I don't put how I take gps coordinates because that was okay I only put which is importent and I dought I think I did mistake here | |
| //thank you | |
| gps_lon_error = l_lon_waypoint - l_lon_gps; //Calculate the latitude error between waypoint and actual position. | |
| gps_lat_error = l_lat_gps - l_lat_waypoint; | |
| gps_lat_total_avarage -= gps_lat_rotating_mem[ gps_rotating_mem_location]; //Subtract the current memory position to make room for the new value. | |
| gps_lat_rotating_mem[ gps_rotating_mem_location] = gps_lat_error - gps_lat_error_previous; //Calculate the new change between the actual pressure and the previous measurement. | |
| gps_lat_total_avarage += gps_lat_rotating_mem[ gps_rotating_mem_location]; //Add the new value to the long term avarage value. | |
| gps_lon_total_avarage -= gps_lon_rotating_mem[ gps_rotating_mem_location]; //Subtract the current memory position to make room for the new value. | |
| gps_lon_rotating_mem[ gps_rotating_mem_location] = gps_lon_error - gps_lon_error_previous; //Calculate the new change between the actual pressure and the previous measurement. | |
| gps_lon_total_avarage += gps_lon_rotating_mem[ gps_rotating_mem_location]; //Add the new value to the long term avarage value. | |
| gps_rotating_mem_location++; //Increase the rotating memory location. | |
| if ( gps_rotating_mem_location == 35) gps_rotating_mem_location = 0; //Start at 0 when the memory location 35 is reached. | |
| gps_lat_error_previous = gps_lat_error; //Remember the error for the next loop. | |
| gps_lon_error_previous = gps_lon_error; //Remember the error for the next loop. | |
| //Calculate the GPS pitch and roll correction as if the nose of the multicopter is facing north. | |
| //The Proportional part = (float)gps_lat_error * gps_p_gain. | |
| //The Derivative part = (float)gps_lat_total_avarage * gps_d_gain | |
| gps_pitch_adjust_north = (float)gps_lat_error * gps_p_gain + (float)gps_lat_total_avarage * gps_d_gain; | |
| gps_roll_adjust_north = (float)gps_lon_error * gps_p_gain + (float)gps_lon_total_avarage * gps_d_gain; | |
| if (!latitude_north)gps_pitch_adjust_north *= -1; //Invert the pitch adjustmet because the quadcopter is flying south of the equator. | |
| if (!longiude_east)gps_roll_adjust_north *= -1; //Invert the roll adjustmet because the quadcopter is flying west of the prime meridian. | |
| //Because the correction is calculated as if the nose was facing north, we need to convert it for the current heading. | |
| gps_roll_adjust = ((float)gps_roll_adjust_north * cos(ANGLE_YAW * 0.017453)) + ((float)gps_pitch_adjust_north * cos((ANGLE_YAW - 90) * 0.017453)); | |
| gps_pitch_adjust = ((float)gps_pitch_adjust_north * cos(ANGLE_YAW * 0.017453)) + ((float)gps_roll_adjust_north * cos((ANGLE_YAW + 90) * 0.017453)); | |
| //Limit the maximum correction to 300. This way we still have full controll with the pitch and roll stick on the transmitter. | |
| if (gps_roll_adjust > 300) gps_roll_adjust = 300; | |
| if (gps_roll_adjust < -300) gps_roll_adjust = -300; | |
| if (gps_pitch_adjust > 300) gps_pitch_adjust = 300; | |
| if (gps_pitch_adjust < -300) gps_pitch_adjust = -300; | |
| if (handling.FLIGHT_MODE >= 3 && waypoint_set == 1) { | |
| PID_ROLL_SETPOINT_BASE += gps_roll_adjust; | |
| PID_PITCH_SETPOINT_BASE += gps_pitch_adjust; | |
| } | |
| //Because we added the GPS adjust values we need to make sure that the control limits are not exceded. | |
| if (PID_ROLL_SETPOINT_BASE > 2000) | |
| PID_ROLL_SETPOINT_BASE = 2000; | |
| if (PID_ROLL_SETPOINT_BASE < 1000) | |
| PID_ROLL_SETPOINT_BASE = 1000; | |
| if (PID_PITCH_SETPOINT_BASE > 2000) | |
| PID_PITCH_SETPOINT_BASE = 2000; | |
| if (PID_PITCH_SETPOINT_BASE < 1000) | |
| PID_PITCH_SETPOINT_BASE = 1000; |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment