Magnetometer Easy Calibration

Projects Programming
18

Here is the portion of the code (used in the video above) that navigates one leg of a course, following a bearing calculated between the current and target GPS coordinates. The magnetometer heading is used for the heading error correction. Steering in this case is twin screws, not rudder.

			gps_last_fix = millis();
			gps_latlon(&lat, &lon);
			gps_parse_enable(); //re-enable parsing

			gps_bearing = (int) (course_to(lat, lon, lat2, lon2, &distance)+0.5);
			dist_to_go = distance + 0.5;

// To take into account wind and currents, 
// substitute calculated GPS bearing for point_bearing

			point_bearing = gps_bearing;
			num_sats = gga_num_sats();

// leg navigation. Have course bearing from GPS coordinates of current loc and finish

		yaw = get_heading();  //read magnetometer
		
		imu_heading = wrap360(yaw + yaw_offset);  //declination correction and compass wrap

// update LCD display
		clear();
		print_long(point_bearing);
		print(" ");
		print_long(imu_heading);
		lcd_goto_xy(1,1);
		print_long(num_sats);
		print("s ");
		print_long(dist_to_go);

// heading error and PID steering 

		error = heading_error(point_bearing, imu_heading);
		
		//error is positive if current_heading > bearing (compass direction)
		//positive bias acts to reduce left motor speed, so bear left

		bias = (kp*error)/10;  //Kp in tenths (Ki, Kd not necessary in water)

		// the motor routines internally limit the argument to {-255, 255}

		set_m1_speed(motorSpeed-bias); //left motor
		set_m2_speed(motorSpeed+bias); //right motor

// check for recent GPS fix

		if (millis() - gps_last_fix > gps_timeout) //5 seconds, currently
			{
			set_motors(0,0); //stop motors
			clear();
			print(" GPS-to"); //timeout!
			printf("E,%lu,5,0,0,0,0\n",millis()); //error dump
			delay(300);//wait a bit
			}
		else {
			if ((millis()-timer) > timeout || dist_to_go < 3) 
			{

// leg timeout or at goal, stop motors

			clear();
			if (dist_to_go < 3) print("DIST");
			else print("TIME");
			set_motors(0,0);
			delay(1000); //drift a bit

			leg++;  //count course legs accomplished
                        }
1 Like