コード例 #1
0
ファイル: gps.cpp プロジェクト: JW-CHOI/Firmware
void
GPS::task_main()
{

	/* open the serial port */
	_serial_fd = ::open(_port, O_RDWR);

	if (_serial_fd < 0) {
		DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno);
		/* tell the dtor that we are exiting, set error code */
		_task = -1;
		_exit(1);
	}

	uint64_t last_rate_measurement = hrt_absolute_time();
	unsigned last_rate_count = 0;

	/* loop handling received serial bytes and also configuring in between */
	while (!_task_should_exit) {

		if (_fake_gps) {
			_report_gps_pos.timestamp_position = hrt_absolute_time();
			_report_gps_pos.lat = (int32_t)47.378301e7f;
			_report_gps_pos.lon = (int32_t)8.538777e7f;
			_report_gps_pos.alt = (int32_t)1200e3f;
			_report_gps_pos.timestamp_variance = hrt_absolute_time();
			_report_gps_pos.s_variance_m_s = 10.0f;
			_report_gps_pos.c_variance_rad = 0.1f;
			_report_gps_pos.fix_type = 3;
			_report_gps_pos.eph = 0.9f;
			_report_gps_pos.epv = 1.8f;
			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
			_report_gps_pos.vel_n_m_s = 0.0f;
			_report_gps_pos.vel_e_m_s = 0.0f;
			_report_gps_pos.vel_d_m_s = 0.0f;
			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
			_report_gps_pos.cog_rad = 0.0f;
			_report_gps_pos.vel_ned_valid = true;				

			/* no time and satellite information simulated */


			if (!(_pub_blocked)) {
				if (_report_gps_pos_pub != nullptr) {
					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);

				} else {
					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
				}
			}

			usleep(2e5);

		} else {

			if (_Helper != nullptr) {
				delete(_Helper);
				/* set to zero to ensure parser is not used while not instantiated */
				_Helper = nullptr;
			}

			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
				break;

			case GPS_DRIVER_MODE_MTK:
				_Helper = new MTK(_serial_fd, &_report_gps_pos);
				break;

			case GPS_DRIVER_MODE_ASHTECH:
				_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
				break;

			default:
				break;
			}

			unlock();

			/* the Ashtech driver lies about successful configuration and the
			 * MTK driver is not well tested, so we really only trust the UBX
			 * driver for an advance publication
			 */
			if (_Helper->configure(_baudrate) == 0) {
				unlock();

				/* reset report */
				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));

				if (_mode == GPS_DRIVER_MODE_UBX) {
					/* Publish initial report that we have access to a GPS,
					 * but set all critical state fields to indicate we have
					 * no valid position lock
					 */

					_report_gps_pos.timestamp_time = hrt_absolute_time();

					/* reset the timestamps for data, because we have no data yet */
					_report_gps_pos.timestamp_position = 0;
					_report_gps_pos.timestamp_variance = 0;
					_report_gps_pos.timestamp_velocity = 0;

					/* set a massive variance */
					_report_gps_pos.eph = 10000.0f;
					_report_gps_pos.epv = 10000.0f;
					_report_gps_pos.fix_type = 0;

					if (!(_pub_blocked)) {
						if (_report_gps_pos_pub != nullptr) {
							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);

						} else {
							_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
						}
					}

					/* GPS is obviously detected successfully, reset statistics */
					_Helper->reset_update_rates();
				}

				int helper_ret;
				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
	//				lock();
					/* opportunistic publishing - else invalid data would end up on the bus */

					if (!(_pub_blocked)) {
						if (helper_ret & 1) {
							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
						}
						if (_p_report_sat_info && (helper_ret & 2)) {
							if (_report_sat_info_pub != nullptr) {
								orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);

							} else {
								_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
							}
						}
					}

					if (helper_ret & 1) {	// consider only pos info updates for rate calculation */
						last_rate_count++;
					}

					/* measure update rate every 5 seconds */
					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
						_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
						last_rate_measurement = hrt_absolute_time();
						last_rate_count = 0;
						_Helper->store_update_rates();
						_Helper->reset_update_rates();
					}

					if (!_healthy) {
						const char *mode_str = "unknown";

						switch (_mode) {
						case GPS_DRIVER_MODE_UBX:
							mode_str = "UBX";
							break;

						case GPS_DRIVER_MODE_MTK:
							mode_str = "MTK";
							break;

						case GPS_DRIVER_MODE_ASHTECH:
							mode_str = "ASHTECH";
							break;

						default:
							break;
						}

						warnx("module found: %s", mode_str);
						_healthy = true;
					}
				}

				if (_healthy) {
					warnx("module lost");
					_healthy = false;
					_rate = 0.0f;
				}

				lock();
			}

			lock();

			/* select next mode */
			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_mode = GPS_DRIVER_MODE_MTK;
				break;

			case GPS_DRIVER_MODE_MTK:
				_mode = GPS_DRIVER_MODE_ASHTECH;
				break;

			case GPS_DRIVER_MODE_ASHTECH:
				_mode = GPS_DRIVER_MODE_UBX;
				break;

			default:
				break;
			}
		}

	}

	warnx("exiting");

	::close(_serial_fd);

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}
コード例 #2
0
ファイル: gps.cpp プロジェクト: dhirajdhule/pandapilot_v4
void
GPS::task_main()
{
	log("starting");

	/* open the serial port */
	_serial_fd = ::open(_port, O_RDWR);

	if (_serial_fd < 0) {
		log("failed to open serial port: %s err: %d", _port, errno);
		/* tell the dtor that we are exiting, set error code */
		_task = -1;
		_exit(1);
	}

	uint64_t last_rate_measurement = hrt_absolute_time();
	unsigned last_rate_count = 0;

	/* loop handling received serial bytes and also configuring in between */
	while (!_task_should_exit) {

		if (_fake_gps) {

			_report_gps_pos.timestamp_position = hrt_absolute_time();
			_report_gps_pos.lat = (int32_t)18.520510e7f;
			_report_gps_pos.lon = (int32_t)73.856733;
			_report_gps_pos.alt = (int32_t)1200e3f;
			_report_gps_pos.timestamp_variance = hrt_absolute_time();
			_report_gps_pos.s_variance_m_s = 10.0f;
			_report_gps_pos.c_variance_rad = 0.1f;
			_report_gps_pos.fix_type = 3;
			_report_gps_pos.eph = 0.9f;
			_report_gps_pos.epv = 1.8f;
			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
			_report_gps_pos.vel_n_m_s = 0.0f;
			_report_gps_pos.vel_e_m_s = 0.0f;
			_report_gps_pos.vel_d_m_s = 0.0f;
			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
			_report_gps_pos.cog_rad = 0.0f;
			_report_gps_pos.vel_ned_valid = true;

			//no time and satellite information simulated

			if (!(_pub_blocked)) {
				if (_report_gps_pos_pub > 0) {
					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);

				} else {
					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
				}
			}

			usleep(2e5);

		} else {

			if (_Helper != nullptr) {
				delete(_Helper);
				/* set to zero to ensure parser is not used while not instantiated */
				_Helper = nullptr;
			}

			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
				break;

			case GPS_DRIVER_MODE_MTK:
				_Helper = new MTK(_serial_fd, &_report_gps_pos);
				break;

			default:
				break;
			}

			unlock();

			if (_Helper->configure(_baudrate) == 0) {
				unlock();

				// GPS is obviously detected successfully, reset statistics
				_Helper->reset_update_rates();

				int helper_ret;
				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
	//				lock();
					/* opportunistic publishing - else invalid data would end up on the bus */

					if (!(_pub_blocked)) {
						if (helper_ret & 1) {
							if (_report_gps_pos_pub > 0) {
								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);

							} else {
								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
							}
						}
						if (_p_report_sat_info && (helper_ret & 2)) {
							if (_report_sat_info_pub > 0) {
								orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);

							} else {
								_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
							}
						}
					}

					if (helper_ret & 1) {	// consider only pos info updates for rate calculation */
						last_rate_count++;
					}

					/* measure update rate every 5 seconds */
					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
						_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
						last_rate_measurement = hrt_absolute_time();
						last_rate_count = 0;
						_Helper->store_update_rates();
						_Helper->reset_update_rates();
					}

					if (!_healthy) {
						const char *mode_str = "unknown";

						switch (_mode) {
						case GPS_DRIVER_MODE_UBX:
							mode_str = "UBX";
							break;

						case GPS_DRIVER_MODE_MTK:
							mode_str = "MTK";
							break;

						default:
							break;
						}

						warnx("module found: %s", mode_str);
						_healthy = true;
					}
				}

				if (_healthy) {
					warnx("module lost");
					_healthy = false;
					_rate = 0.0f;
				}

				lock();
			}

			lock();

			/* select next mode */
			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_mode = GPS_DRIVER_MODE_MTK;
				break;

			case GPS_DRIVER_MODE_MTK:
				_mode = GPS_DRIVER_MODE_UBX;
				break;

			default:
				break;
			}
		}

	}

	warnx("exiting");

	::close(_serial_fd);

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}
コード例 #3
0
ファイル: gps.cpp プロジェクト: ducardg/Firmware
void
GPS::task_main()
{
	log("starting");

	/* open the serial port */
	_serial_fd = ::open(_port, O_RDWR);

	if (_serial_fd < 0) {
		log("failed to open serial port: %s err: %d", _port, errno);
		/* tell the dtor that we are exiting, set error code */
		_task = -1;
		_exit(1);
	}

	uint64_t last_rate_measurement = hrt_absolute_time();
	unsigned last_rate_count = 0;

	/* loop handling received serial bytes and also configuring in between */
	while (!_task_should_exit) {

		if (_Helper != nullptr) {
			delete(_Helper);
			/* set to zero to ensure parser is not used while not instantiated */
			_Helper = nullptr;
		}

		switch (_mode) {
		case GPS_DRIVER_MODE_UBX:
			_Helper = new UBX(_serial_fd, &_report);
			break;

		case GPS_DRIVER_MODE_MTK:
			_Helper = new MTK(_serial_fd, &_report);
			break;

		case GPS_DRIVER_MODE_NMEA:
			//_Helper = new NMEA(); //TODO: add NMEA
			break;

		default:
			break;
		}

		unlock();

		if (_Helper->configure(_baudrate) == 0) {
			unlock();

			// GPS is obviously detected successfully, reset statistics
			_Helper->reset_update_rates();

			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
//				lock();
				/* opportunistic publishing - else invalid data would end up on the bus */
				if (_report_pub > 0) {
					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);

				} else {
					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
				}

				last_rate_count++;

				/* measure update rate every 5 seconds */
				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
					last_rate_measurement = hrt_absolute_time();
					last_rate_count = 0;
					_Helper->store_update_rates();
					_Helper->reset_update_rates();
				}

				if (!_healthy) {
					char *mode_str = "unknown";

					switch (_mode) {
					case GPS_DRIVER_MODE_UBX:
						mode_str = "UBX";
						break;

					case GPS_DRIVER_MODE_MTK:
						mode_str = "MTK";
						break;

					case GPS_DRIVER_MODE_NMEA:
						mode_str = "NMEA";
						break;

					default:
						break;
					}

					warnx("module found: %s", mode_str);
					_healthy = true;
				}
			}

			if (_healthy) {
				warnx("module lost");
				_healthy = false;
				_rate = 0.0f;
			}

			lock();
		}

		lock();

		/* select next mode */
		switch (_mode) {
		case GPS_DRIVER_MODE_UBX:
			_mode = GPS_DRIVER_MODE_MTK;
			break;

		case GPS_DRIVER_MODE_MTK:
			_mode = GPS_DRIVER_MODE_UBX;
			break;

			//				case GPS_DRIVER_MODE_NMEA:
			//					_mode = GPS_DRIVER_MODE_UBX;
			//					break;
		default:
			break;
		}

	}

	warnx("exiting");

	::close(_serial_fd);

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}