Exemplo n.º 1
0
static void simulateModelAgnostic()
{
	float Rbe[3][3];
	float q[4];

	// Simulate accels based on current attitude
	AttitudeActualData attitudeActual;
	AttitudeActualGet(&attitudeActual);
	q[0] = attitudeActual.q1;
	q[1] = attitudeActual.q2;
	q[2] = attitudeActual.q3;
	q[3] = attitudeActual.q4;
	Quaternion2R(q,Rbe);

	AccelsData accelsData; // Skip get as we set all the fields
	accelsData.x = -GRAVITY * Rbe[0][2];
	accelsData.y = -GRAVITY * Rbe[1][2];
	accelsData.z = -GRAVITY * Rbe[2][2];
	accelsData.temperature = 30;
	AccelsSet(&accelsData);

	RateDesiredData rateDesired;
	RateDesiredGet(&rateDesired);

	GyrosData gyrosData; // Skip get as we set all the fields
	gyrosData.x = rateDesired.Roll + rand_gauss();
	gyrosData.y = rateDesired.Pitch + rand_gauss();
	gyrosData.z = rateDesired.Yaw + rand_gauss();

	// Apply bias correction to the gyros
	GyrosBiasData gyrosBias;
	GyrosBiasGet(&gyrosBias);
	gyrosData.x += gyrosBias.x;
	gyrosData.y += gyrosBias.y;
	gyrosData.z += gyrosBias.z;

	GyrosSet(&gyrosData);

	BaroAltitudeData baroAltitude;
	BaroAltitudeGet(&baroAltitude);
	baroAltitude.Altitude = 1;
	BaroAltitudeSet(&baroAltitude);

	GPSPositionData gpsPosition;
	GPSPositionGet(&gpsPosition);
	gpsPosition.Latitude = 0;
	gpsPosition.Longitude = 0;
	gpsPosition.Altitude = 0;
	GPSPositionSet(&gpsPosition);

	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
	// and make it average zero (weakly)
	MagnetometerData mag;
	mag.x = 400;
	mag.y = 0;
	mag.z = 800;
	MagnetometerSet(&mag);
}
Exemplo n.º 2
0
/* library functions */
void GPSPosition_Get(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
	if (Param[0]->Val->Pointer == NULL)
	return;

	GPSPositionData data;
	GPSPositionGet(&data);

	// use the same struct like picoc. see below
#ifndef NO_FP
	struct mystruct {
		long Latitude;
		long Longitude;
		double Altitude;
		double GeoidSeparation;
		double Heading;
		double Groundspeed;
		double PDOP;
		double HDOP;
		double VDOP;
		unsigned char Status;
		char Satellites;
	} *pdata;
	pdata = Param[0]->Val->Pointer;
	pdata->Latitude = data.Latitude;
	pdata->Longitude = data.Longitude;
	pdata->Altitude = data.Altitude;
	pdata->GeoidSeparation = data.GeoidSeparation;
	pdata->Heading = data.Heading;
	pdata->Groundspeed = data.Groundspeed;
	pdata->PDOP = data.PDOP;
	pdata->HDOP = data.HDOP;
	pdata->VDOP = data.VDOP;
	pdata->Status = data.Status;
	pdata->Satellites = data.Satellites;
#else
	struct mystruct {
		long Latitude;
		long Longitude;
		unsigned char Status;
		char Satellites;
	} *pdata;
	pdata = Param[0]->Val->Pointer;
	pdata->Latitude = data.Latitude;
	pdata->Longitude = data.Longitude;
	pdata->Status = data.Status;
	pdata->Satellites = data.Satellites;
#endif
}
Exemplo n.º 3
0
static void simulateConstant()
{
	AccelsData accelsData; // Skip get as we set all the fields
	accelsData.x = 0;
	accelsData.y = 0;
	accelsData.z = -GRAVITY;
	accelsData.temperature = 0;
	AccelsSet(&accelsData);

	GyrosData gyrosData; // Skip get as we set all the fields
	gyrosData.x = 0;
	gyrosData.y = 0;
	gyrosData.z = 0;

	// Apply bias correction to the gyros
	GyrosBiasData gyrosBias;
	GyrosBiasGet(&gyrosBias);
	gyrosData.x += gyrosBias.x;
	gyrosData.y += gyrosBias.y;
	gyrosData.z += gyrosBias.z;

	GyrosSet(&gyrosData);

	BaroAltitudeData baroAltitude;
	BaroAltitudeGet(&baroAltitude);
	baroAltitude.Altitude = 1;
	BaroAltitudeSet(&baroAltitude);

	GPSPositionData gpsPosition;
	GPSPositionGet(&gpsPosition);
	gpsPosition.Latitude = 0;
	gpsPosition.Longitude = 0;
	gpsPosition.Altitude = 0;
	GPSPositionSet(&gpsPosition);

	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
	// and make it average zero (weakly)
	MagnetometerData mag;
	mag.x = 400;
	mag.y = 0;
	mag.z = 800;
	MagnetometerSet(&mag);
}
Exemplo n.º 4
0
static void uavoMavlinkBridgeTask(void *parameters) {
	uint32_t lastSysTime;
	// Main task loop
	lastSysTime = PIOS_Thread_Systime();

	FlightBatterySettingsData batSettings = {};

	if (FlightBatterySettingsHandle() != NULL )
		FlightBatterySettingsGet(&batSettings);

	SystemStatsData systemStats;

	while (1) {
		PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ);

		if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
			FlightBatteryStateData batState = {};

			if (FlightBatteryStateHandle() != NULL )
				FlightBatteryStateGet(&batState);

			SystemStatsGet(&systemStats);

			int8_t battery_remaining = 0;
			if (batSettings.Capacity != 0) {
				if (batState.ConsumedEnergy < batSettings.Capacity) {
					battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100);
				}
			}

			uint16_t voltage = 0;
			if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
				voltage = lroundf(batState.Voltage * 1000);

			uint16_t current = 0;
			if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
				current = lroundf(batState.Current * 100);

			mavlink_msg_sys_status_pack(0, 200, mav_msg,
					// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
					0,
					// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
					0,
					// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
					0,
					// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
					(uint16_t)systemStats.CPULoad * 10,
					// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
					voltage,
					// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
					current,
					// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
					battery_remaining,
					// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
					0,
					// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
					0,
					// errors_count1 Autopilot-specific errors
					0,
					// errors_count2 Autopilot-specific errors
					0,
					// errors_count3 Autopilot-specific errors
					0,
					// errors_count4 Autopilot-specific errors
					0);

			send_message();
		}

		if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) {
			ManualControlCommandData manualState;
			FlightStatusData flightStatus;

			ManualControlCommandGet(&manualState);
			FlightStatusGet(&flightStatus);
			SystemStatsGet(&systemStats);

			//TODO connect with RSSI object and pass in last argument
			mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
					// time_boot_ms Timestamp (milliseconds since system boot)
					systemStats.FlightTime,
					// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
					0,
					// chan1_raw RC channel 1 value, in microseconds
					manualState.Channel[0],
					// chan2_raw RC channel 2 value, in microseconds
					manualState.Channel[1],
					// chan3_raw RC channel 3 value, in microseconds
					manualState.Channel[2],
					// chan4_raw RC channel 4 value, in microseconds
					manualState.Channel[3],
					// chan5_raw RC channel 5 value, in microseconds
					manualState.Channel[4],
					// chan6_raw RC channel 6 value, in microseconds
					manualState.Channel[5],
					// chan7_raw RC channel 7 value, in microseconds
					manualState.Channel[6],
					// chan8_raw RC channel 8 value, in microseconds
					manualState.Channel[7],
					// rssi Receive signal strength indicator, 0: 0%, 255: 100%
					manualState.Rssi);

			send_message();
		}

		if (stream_trigger(MAV_DATA_STREAM_POSITION)) {
			GPSPositionData gpsPosData = {};
			HomeLocationData homeLocation = {};
			SystemStatsGet(&systemStats);

			if (GPSPositionHandle() != NULL )
				GPSPositionGet(&gpsPosData);
			if (HomeLocationHandle() != NULL )
				HomeLocationGet(&homeLocation);
			SystemStatsGet(&systemStats);

			uint8_t gps_fix_type;
			switch (gpsPosData.Status)
			{
			case GPSPOSITION_STATUS_NOGPS:
				gps_fix_type = 0;
				break;
			case GPSPOSITION_STATUS_NOFIX:
				gps_fix_type = 1;
				break;
			case GPSPOSITION_STATUS_FIX2D:
				gps_fix_type = 2;
				break;
			case GPSPOSITION_STATUS_FIX3D:
			case GPSPOSITION_STATUS_DIFF3D:
				gps_fix_type = 3;
				break;
			default:
				gps_fix_type = 0;
				break;
			}

			mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
					// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
					(uint64_t)systemStats.FlightTime * 1000,
					// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
					gps_fix_type,
					// lat Latitude in 1E7 degrees
					gpsPosData.Latitude,
					// lon Longitude in 1E7 degrees
					gpsPosData.Longitude,
					// alt Altitude in 1E3 meters (millimeters) above MSL
					gpsPosData.Altitude * 1000,
					// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
					gpsPosData.HDOP * 100,
					// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
					gpsPosData.VDOP * 100,
					// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
					gpsPosData.Groundspeed * 100,
					// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
					gpsPosData.Heading * 100,
					// satellites_visible Number of satellites visible. If unknown, set to 255
					gpsPosData.Satellites);

			send_message();

			mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
					// latitude Latitude (WGS84), expressed as * 1E7
					homeLocation.Latitude,
					// longitude Longitude (WGS84), expressed as * 1E7
					homeLocation.Longitude,
					// altitude Altitude(WGS84), expressed as * 1000
					homeLocation.Altitude * 1000);

			send_message();

			//TODO add waypoint nav stuff
			//wp_target_bearing
			//wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
			//alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
			//aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
			//xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
			//mavlink_msg_nav_controller_output_pack
			//wp_number
			//mavlink_msg_mission_current_pack
		}

		if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) {
			AttitudeActualData attActual;
			SystemStatsData systemStats;

			AttitudeActualGet(&attActual);
			SystemStatsGet(&systemStats);

			mavlink_msg_attitude_pack(0, 200, mav_msg,
					// time_boot_ms Timestamp (milliseconds since system boot)
					systemStats.FlightTime,
					// roll Roll angle (rad)
					attActual.Roll * DEG2RAD,
					// pitch Pitch angle (rad)
					attActual.Pitch * DEG2RAD,
					// yaw Yaw angle (rad)
					attActual.Yaw * DEG2RAD,
					// rollspeed Roll angular speed (rad/s)
					0,
					// pitchspeed Pitch angular speed (rad/s)
					0,
					// yawspeed Yaw angular speed (rad/s)
					0);

			send_message();
		}

		if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) {
			ActuatorDesiredData actDesired;
			AttitudeActualData attActual;
			AirspeedActualData airspeedActual = {};
			GPSPositionData gpsPosData = {};
			BaroAltitudeData baroAltitude = {};
			FlightStatusData flightStatus;

			if (AirspeedActualHandle() != NULL )
				AirspeedActualGet(&airspeedActual);
			if (GPSPositionHandle() != NULL )
				GPSPositionGet(&gpsPosData);
			if (BaroAltitudeHandle() != NULL )
				BaroAltitudeGet(&baroAltitude);
			ActuatorDesiredGet(&actDesired);
			AttitudeActualGet(&attActual);
			FlightStatusGet(&flightStatus);

			float altitude = 0;
			if (BaroAltitudeHandle() != NULL)
				altitude = baroAltitude.Altitude;
			else if (GPSPositionHandle() != NULL)
				altitude = gpsPosData.Altitude;

			// round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
			int16_t heading = lroundf(attActual.Yaw);
			if (heading < 0)
				heading += 360;

			mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
					// airspeed Current airspeed in m/s
					airspeedActual.TrueAirspeed,
					// groundspeed Current ground speed in m/s
					gpsPosData.Groundspeed,
					// heading Current heading in degrees, in compass units (0..360, 0=north)
					heading,
					// throttle Current throttle setting in integer percent, 0 to 100
					actDesired.Throttle * 100,
					// alt Current altitude (MSL), in meters
					altitude,
					// climb Current climb rate in meters/second
					0);

			send_message();

			uint8_t armed_mode = 0;
			if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED)
				armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;

			uint8_t custom_mode = CUSTOM_MODE_STAB;

			switch (flightStatus.FlightMode) {
				case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
				case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
				case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
				case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
					/* Kinda a catch all */
					custom_mode = CUSTOM_MODE_SPORT;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_ACRO:
				case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
					custom_mode = CUSTOM_MODE_ACRO;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
				case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
				case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
					/* May want these three to try and
					 * infer based on roll axis */
				case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
					custom_mode = CUSTOM_MODE_STAB;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
					custom_mode = CUSTOM_MODE_DRIFT;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
					custom_mode = CUSTOM_MODE_ALTH;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
					custom_mode = CUSTOM_MODE_RTL;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
				case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
					custom_mode = CUSTOM_MODE_POSH;
					break;
				case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
					custom_mode = CUSTOM_MODE_AUTO;
					break;
			}

			mavlink_msg_heartbeat_pack(0, 200, mav_msg,
					// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
					MAV_TYPE_GENERIC,
					// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
					MAV_AUTOPILOT_GENERIC,
					// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
					armed_mode,
					// custom_mode A bitfield for use for autopilot-specific flags.
					custom_mode,
					// system_status System status flag, see MAV_STATE ENUM
					0);

			send_message();
		}
	}
}
Exemplo n.º 5
0
static void gpsTask(void *parameters)
{
	portTickType xDelay = 100 / portTICK_RATE_MS;
	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
	GPSPositionData GpsData;
	
#ifdef ENABLE_GPS_BINARY_GTOP
	GTOP_BIN_init();
#else
	uint8_t rx_count = 0;
	bool start_flag = false;
	bool found_cr = false;
	int32_t gpsRxOverflow = 0;
#endif
	
#ifdef FULL_COLD_RESTART
	// tell the GPS to do a FULL COLD restart
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n");
	timeOfLastCommandMs = timeNowMs;
	while (timeNowMs - timeOfLastCommandMs < 300)	// delay for 300ms to let the GPS sort itself out
	{
		vTaskDelay(xDelay);	// Block task until next update
		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
	}
#endif

#ifdef DISABLE_GPS_TRESHOLD
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n");
#endif

#ifdef ENABLE_GPS_BINARY_GTOP
	// switch to GTOP binary mode
	PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
#endif
	
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
	// switch to single sentence mode
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n");
#endif

#ifdef ENABLE_GPS_NMEA
	// switch to NMEA mode
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n");
#endif

	numUpdates = 0;
	numChecksumErrors = 0;
	numParsingErrors = 0;

	timeOfLastUpdateMs = timeNowMs;
	timeOfLastCommandMs = timeNowMs;

	// Loop forever
	while (1)
	{
		#ifdef ENABLE_GPS_BINARY_GTOP
			// GTOP BINARY GPS mode

			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
			{
				int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
				if (res >= 0)
				{
					numUpdates++;

					timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
					timeOfLastUpdateMs = timeNowMs;
					timeOfLastCommandMs = timeNowMs;
				}
			}

		#else
			// NMEA or SINGLE-SENTENCE GPS mode

			// This blocks the task until there is something on the buffer
			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
			{
				char c = PIOS_COM_ReceiveBuffer(gpsPort);
			
				// detect start while acquiring stream
				if (!start_flag && (c == '$'))
				{
					start_flag = true;
					found_cr = false;
					rx_count = 0;
				}
				else
				if (!start_flag)
					continue;
			
				if (rx_count >= sizeof(gps_rx_buffer))
				{
					// The buffer is already full and we haven't found a valid NMEA sentence.
					// Flush the buffer and note the overflow event.
					gpsRxOverflow++;
					start_flag = false;
					found_cr = false;
					rx_count = 0;
				}
				else
				{
					gps_rx_buffer[rx_count] = c;
					rx_count++;
				}
			
				// look for ending '\r\n' sequence
				if (!found_cr && (c == '\r') )
					found_cr = true;
				else
				if (found_cr && (c != '\n') )
					found_cr = false;  // false end flag
				else
				if (found_cr && (c == '\n') )
				{
					// The NMEA functions require a zero-terminated string
					// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
					gps_rx_buffer[rx_count-2] = 0;

					// prepare to parse next sentence
					start_flag = false;
					found_cr = false;
					rx_count = 0;
					// Our rxBuffer must look like this now:
					//   [0]           = '$'
					//   ...           = zero or more bytes of sentence payload
					//   [end_pos - 1] = '\r'
					//   [end_pos]     = '\n'
					//
					// Prepare to consume the sentence from the buffer
				
					// Validate the checksum over the sentence
					if (!NMEA_checksum(&gps_rx_buffer[1]))
					{	// Invalid checksum.  May indicate dropped characters on Rx.
						PIOS_DEBUG_PinHigh(2);
						++numChecksumErrors;
						PIOS_DEBUG_PinLow(2);
					}
					else
					{	// Valid checksum, use this packet to update the GPS position
						if (!NMEA_update_position(&gps_rx_buffer[1])) {
							PIOS_DEBUG_PinHigh(2);
							++numParsingErrors;
							PIOS_DEBUG_PinLow(2);
						}
						else
							++numUpdates;

						timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
						timeOfLastUpdateMs = timeNowMs;
						timeOfLastCommandMs = timeNowMs;
					}
				}
			}
		#endif

		// Check for GPS timeout
		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
		{	// we have not received any valid GPS sentences for a while.
			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.

			GPSPositionGet(&GpsData);
			GpsData.Status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionSet(&GpsData);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);

			if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)
			{	// resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command
				timeOfLastCommandMs = timeNowMs;

				#ifdef ENABLE_GPS_BINARY_GTOP
					GTOP_BIN_init();
					// switch to binary mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n");
				#endif

				#ifdef ENABLE_GPS_ONESENTENCE_GTOP
					// switch to single sentence mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n");
				#endif

				#ifdef ENABLE_GPS_NMEA
					// switch to NMEA mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
				#endif

				#ifdef DISABLE_GPS_TRESHOLD
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n");
				#endif
			}
		}
		else
		{	// we appear to be receiving GPS sentences OK, we've had an update

			HomeLocationData home;
			HomeLocationGet(&home);

			GPSPositionGet(&GpsData);
			if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
				setHomeLocation(&GpsData);

			//criteria for GPS-OK taken from this post...
			//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
			if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			else
			if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
			else
				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

		// Block task until next update
		vTaskDelay(xDelay);
	}
}
Exemplo n.º 6
0
Arquivo: GPS.c Projeto: 1heinz/TauLabs
static void gpsTask(void *parameters)
{
	portTickType xDelay = MS2TICKS(GPS_COM_TIMEOUT_MS);
	uint32_t timeNowMs = TICKS2MS(xTaskGetTickCount());

	GPSPositionData gpsposition;
	uint8_t	gpsProtocol;

	ModuleSettingsGPSDataProtocolGet(&gpsProtocol);

#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
	gps_airspeed_initialize();
#endif

	timeOfLastUpdateMs = timeNowMs;
	timeOfLastCommandMs = timeNowMs;


#if !defined(PIOS_GPS_MINIMAL)
	switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
		case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
		{
			uint8_t gpsAutoConfigure;
			ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);

			if (gpsAutoConfigure == MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {

				// Wait for power to stabilize before talking to external devices
				vTaskDelay(MS2TICKS(1000));

				// Runs through a number of possible GPS baud rates to
				// configure the ublox baud rate. This uses a NMEA string
				// so could work for either UBX or NMEA actually. This is
				// somewhat redundant with updateSettings below, but that
				// is only called on startup and is not an issue.
				ModuleSettingsGPSSpeedOptions baud_rate;
				ModuleSettingsGPSSpeedGet(&baud_rate);
				ubx_cfg_set_baudrate(gpsPort, baud_rate);

				vTaskDelay(MS2TICKS(1000));

				ubx_cfg_send_configuration(gpsPort, gps_rx_buffer);
			}
		}
			break;
#endif
	}
#endif /* PIOS_GPS_MINIMAL */

	GPSPositionGet(&gpsposition);
	// Loop forever
	while (1)
	{
		uint8_t c;

		// This blocks the task until there is something on the buffer
		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
		{
			int res;
			switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
					res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
					res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
				default:
					res = NO_PARSER; // this should not happen
					break;
			}

			if (res == PARSER_COMPLETE) {
				timeNowMs = TICKS2MS(xTaskGetTickCount());
				timeOfLastUpdateMs = timeNowMs;
				timeOfLastCommandMs = timeNowMs;
			}
		}

		// Check for GPS timeout
		timeNowMs = TICKS2MS(xTaskGetTickCount());
		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
			// we have not received any valid GPS sentences for a while.
			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
			uint8_t status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionStatusSet(&status);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
		} else {
			// we appear to be receiving GPS sentences OK, we've had an update
			//criteria for GPS-OK taken from this post
			if (gpsposition.PDOP < 3.5f && 
			    gpsposition.Satellites >= 7 &&
			    (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			         gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			           gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
					else
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

	}
}
void GPSPositionLongitude(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
	GPSPositionData data;
	GPSPositionGet(&data);
	ReturnValue->Val->LongInteger = data.Longitude;
}
Exemplo n.º 8
0
static void gpsTask(void *parameters)
{
	GPSPositionData gpsposition;

	uint32_t timeOfLastUpdateMs = 0;
	uint32_t timeOfConfigAttemptMs = 0;

	uint8_t	gpsProtocol;

#ifdef PIOS_GPS_PROVIDES_AIRSPEED
	gps_airspeed_initialize();
#endif

	GPSPositionGet(&gpsposition);

	// Wait for power to stabilize before talking to external devices
	PIOS_Thread_Sleep(1000);

	// Loop forever
	while (1) {
		uint32_t xDelay = GPS_COM_TIMEOUT_MS;

		uint32_t loopTimeMs = PIOS_Thread_Systime();

		// XXX TODO: also on modulesettings change..
		if (!timeOfConfigAttemptMs) {
			ModuleSettingsGPSDataProtocolGet(&gpsProtocol);

			gpsConfigure(gpsProtocol);
			timeOfConfigAttemptMs = PIOS_Thread_Systime();

			continue;
		}

		uint8_t c;

		// This blocks the task until there is something on the buffer
		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
		{
			int res;
			switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
					res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
					res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
				default:
					res = NO_PARSER; // this should not happen
					break;
			}

			if (res == PARSER_COMPLETE) {
				timeOfLastUpdateMs = loopTimeMs;
			}

			xDelay = 0;	// For now on, don't block / wait,
					// but consume what we can from the fifo
		}

		// Check for GPS timeout
		if ((loopTimeMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
			// we have not received any valid GPS sentences for a while.
			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
			uint8_t status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionStatusSet(&status);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
			/* Don't reinitialize too often. */
			if ((loopTimeMs - timeOfConfigAttemptMs) >= GPS_TIMEOUT_MS) {
				timeOfConfigAttemptMs = 0; // reinit next loop
			}
		} else {
			// we appear to be receiving GPS sentences OK, we've had an update
			//criteria for GPS-OK taken from this post
			if (gpsposition.PDOP < 3.5f && 
			    gpsposition.Satellites >= 7 &&
			    (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			         gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			           gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
					else
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

	}
}
Exemplo n.º 9
0
/**
 * This method performs a simple simulation of a car
 * 
 * It takes in the ActuatorDesired command to rotate the aircraft and performs
 * a simple kinetic model where the throttle increases the energy and drag decreases
 * it.  Changing altitude moves energy from kinetic to potential.
 *
 * 1. Update attitude based on ActuatorDesired
 * 2. Update position based on velocity
 */
static void simulateModelCar()
{
	static double pos[3] = {0,0,0};
	static double vel[3] = {0,0,0};
	static double ned_accel[3] = {0,0,0};
	static float q[4] = {1,0,0,0};
	static float rpy[3] = {0,0,0}; // Low pass filtered actuator
	static float baro_offset = 0.0f;
	float Rbe[3][3];
	
	const float ACTUATOR_ALPHA = 0.8;
	const float MAX_THRUST = 9.81 * 0.5;
	const float K_FRICTION = 0.2;
	const float GPS_PERIOD = 0.1;
	const float MAG_PERIOD = 1.0 / 75.0;
	const float BARO_PERIOD = 1.0 / 20.0;
	
	static uint32_t last_time;
	
	float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
	if(dT < 1e-3)
		dT = 2e-3;
	last_time = PIOS_DELAY_GetRaw();
	
	FlightStatusData flightStatus;
	FlightStatusGet(&flightStatus);
	ActuatorDesiredData actuatorDesired;
	ActuatorDesiredGet(&actuatorDesired);
	
	float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
	if (thrust < 0)
		thrust = 0;
	
	if (thrust != thrust)
		thrust = 0;
	
	//	float control_scaling = thrust * thrustToDegs;
	//	// In rad/s
	//	rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
	//	rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
	//	rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
	//	
	//	GyrosData gyrosData; // Skip get as we set all the fields
	//	gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
	//	gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
	//	gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
	
	/**** 1. Update attitude ****/
	RateDesiredData rateDesired;
	RateDesiredGet(&rateDesired);
	
	// Need to get roll angle for easy cross coupling
	AttitudeActualData attitudeActual;
	AttitudeActualGet(&attitudeActual);

	rpy[0] = 0; // cannot roll
	rpy[1] = 0; // cannot pitch
	rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
	

	GyrosData gyrosData; // Skip get as we set all the fields
	gyrosData.x = rpy[0] + rand_gauss();
	gyrosData.y = rpy[1] + rand_gauss();
	gyrosData.z = rpy[2] + rand_gauss();
	GyrosSet(&gyrosData);
	
	// Predict the attitude forward in time
	float qdot[4];
	qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
	
	// Take a time step
	q[0] = q[0] + qdot[0];
	q[1] = q[1] + qdot[1];
	q[2] = q[2] + qdot[2];
	q[3] = q[3] + qdot[3];
	
	float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	q[0] = q[0] / qmag;
	q[1] = q[1] / qmag;
	q[2] = q[2] / qmag;
	q[3] = q[3] / qmag;
	
	if(overideAttitude){
		AttitudeActualData attitudeActual;
		AttitudeActualGet(&attitudeActual);
		attitudeActual.q1 = q[0];
		attitudeActual.q2 = q[1];
		attitudeActual.q3 = q[2];
		attitudeActual.q4 = q[3];
		AttitudeActualSet(&attitudeActual);
	}
	
	/**** 2. Update position based on velocity ****/
	// Rbe takes a vector from body to earth.  If we take (1,0,0)^T through this and then dot with airspeed
	// we get forward airspeed		
	Quaternion2R(q,Rbe);

	double groundspeed[3] = {vel[0], vel[1], vel[2] };
	double forwardSpeed = Rbe[0][0] * groundspeed[0] + Rbe[0][1] * groundspeed[1] + Rbe[0][2] * groundspeed[2];
	double sidewaysSpeed = Rbe[1][0] * groundspeed[0] + Rbe[1][1] * groundspeed[1] + Rbe[1][2] * groundspeed[2];

	/* Compute aerodynamic forces in body referenced frame.  Later use more sophisticated equations  */
	/* TODO: This should become more accurate.  Use the force equations to calculate lift from the   */
	/* various surfaces based on AoA and airspeed.  From that compute torques and forces.  For later */
	double forces[3]; // X, Y, Z
	forces[0] = thrust - forwardSpeed * K_FRICTION;         // Friction is applied in all directions in NED
	forces[1] = 0 - sidewaysSpeed * K_FRICTION * 100;      // No side slip
	forces[2] = 0;
	
	// Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
	ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
	ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
	ned_accel[2] = 0;

	// Apply acceleration based on velocity
	ned_accel[0] -= K_FRICTION * (vel[0]);
	ned_accel[1] -= K_FRICTION * (vel[1]);
	
	// Predict the velocity forward in time
	vel[0] = vel[0] + ned_accel[0] * dT;
	vel[1] = vel[1] + ned_accel[1] * dT;
	vel[2] = vel[2] + ned_accel[2] * dT;
	
	// Predict the position forward in time
	pos[0] = pos[0] + vel[0] * dT;
	pos[1] = pos[1] + vel[1] * dT;
	pos[2] = pos[2] + vel[2] * dT;
	
	// Simulate hitting ground
	if(pos[2] > 0) {
		pos[2] = 0;
		vel[2] = 0;
		ned_accel[2] = 0;
	}
	
	// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
	ned_accel[2] -= GRAVITY;
	
	// Transform the accels back in to body frame
	AccelsData accelsData; // Skip get as we set all the fields
	accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
	accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
	accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
	accelsData.temperature = 30;
	AccelsSet(&accelsData);
	
	if(baro_offset == 0) {
		// Hacky initialization
		baro_offset = 50;// * rand_gauss();
	} else {
		// Very small drift process
		baro_offset += rand_gauss() / 100;
	}
	// Update baro periodically	
	static uint32_t last_baro_time = 0;
	if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
		BaroAltitudeData baroAltitude;
		BaroAltitudeGet(&baroAltitude);
		baroAltitude.Altitude = -pos[2] + baro_offset;
		BaroAltitudeSet(&baroAltitude);
		last_baro_time = PIOS_DELAY_GetRaw();
	}
	
	HomeLocationData homeLocation;
	HomeLocationGet(&homeLocation);
	
	static float gps_vel_drift[3] = {0,0,0};
	gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
	gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
	gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
	
	// Update GPS periodically	
	static uint32_t last_gps_time = 0;
	if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
		// Use double precision here as simulating what GPS produces
		double T[3];
		T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
		T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
		T[2] = -1.0;
		
		static float gps_drift[3] = {0,0,0};
		gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
		gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
		gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
		
		GPSPositionData gpsPosition;
		GPSPositionGet(&gpsPosition);
		gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
		gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
		gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
		gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
		gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
		gpsPosition.Satellites = 7;
		gpsPosition.PDOP = 1;
		GPSPositionSet(&gpsPosition);
		last_gps_time = PIOS_DELAY_GetRaw();
	}
	
	// Update GPS Velocity measurements
	static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
	if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
		GPSVelocityData gpsVelocity;
		GPSVelocityGet(&gpsVelocity);
		gpsVelocity.North = vel[0] + gps_vel_drift[0];
		gpsVelocity.East = vel[1] + gps_vel_drift[1];
		gpsVelocity.Down = vel[2] + gps_vel_drift[2];
		GPSVelocitySet(&gpsVelocity);
		last_gps_vel_time = PIOS_DELAY_GetRaw();
	}
	
	// Update mag periodically
	static uint32_t last_mag_time = 0;
	if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
		MagnetometerData mag;
		mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
		mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
		mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
		magOffsetEstimation(&mag);
		MagnetometerSet(&mag);
		last_mag_time = PIOS_DELAY_GetRaw();
	}
	
	AttitudeSimulatedData attitudeSimulated;
	AttitudeSimulatedGet(&attitudeSimulated);
	attitudeSimulated.q1 = q[0];
	attitudeSimulated.q2 = q[1];
	attitudeSimulated.q3 = q[2];
	attitudeSimulated.q4 = q[3];
	Quaternion2RPY(q,&attitudeSimulated.Roll);
	attitudeSimulated.Position[0] = pos[0];
	attitudeSimulated.Position[1] = pos[1];
	attitudeSimulated.Position[2] = pos[2];
	attitudeSimulated.Velocity[0] = vel[0];
	attitudeSimulated.Velocity[1] = vel[1];
	attitudeSimulated.Velocity[2] = vel[2];
	AttitudeSimulatedSet(&attitudeSimulated);
}
Exemplo n.º 10
0
static void simulateModelQuadcopter()
{
	static double pos[3] = {0,0,0};
	static double vel[3] = {0,0,0};
	static double ned_accel[3] = {0,0,0};
	static float q[4] = {1,0,0,0};
	static float rpy[3] = {0,0,0}; // Low pass filtered actuator
	static float baro_offset = 0.0f;
	static float temperature = 20;
	float Rbe[3][3];
	
	const float ACTUATOR_ALPHA = 0.8;
	const float MAX_THRUST = GRAVITY * 2;
	const float K_FRICTION = 1;
	const float GPS_PERIOD = 0.1;
	const float MAG_PERIOD = 1.0 / 75.0;
	const float BARO_PERIOD = 1.0 / 20.0;
	
	static uint32_t last_time;
	
	float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
	if(dT < 1e-3)
		dT = 2e-3;
	last_time = PIOS_DELAY_GetRaw();
	
	FlightStatusData flightStatus;
	FlightStatusGet(&flightStatus);
	ActuatorDesiredData actuatorDesired;
	ActuatorDesiredGet(&actuatorDesired);

	float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
	if (thrust < 0)
		thrust = 0;
	
	if (thrust != thrust)
		thrust = 0;
	
//	float control_scaling = thrust * thrustToDegs;
//	// In rad/s
//	rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
//	rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
//	rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//	
//	GyrosData gyrosData; // Skip get as we set all the fields
//	gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
//	gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
//	gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
	
	RateDesiredData rateDesired;
	RateDesiredGet(&rateDesired);
	
	rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
	rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
	rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
	

	temperature = 20;
	GyrosData gyrosData; // Skip get as we set all the fields
	gyrosData.x = rpy[0] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11; // - powf(temperature - 20,3) * 0.05;;
	gyrosData.y = rpy[1] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
	gyrosData.z = rpy[2] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
	gyrosData.temperature = temperature;
	GyrosSet(&gyrosData);
	
	// Predict the attitude forward in time
	float qdot[4];
	qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
	qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
	
	// Take a time step
	q[0] = q[0] + qdot[0];
	q[1] = q[1] + qdot[1];
	q[2] = q[2] + qdot[2];
	q[3] = q[3] + qdot[3];
	
	float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	q[0] = q[0] / qmag;
	q[1] = q[1] / qmag;
	q[2] = q[2] / qmag;
	q[3] = q[3] / qmag;
	
	if(overideAttitude){
		AttitudeActualData attitudeActual;
		AttitudeActualGet(&attitudeActual);
		attitudeActual.q1 = q[0];
		attitudeActual.q2 = q[1];
		attitudeActual.q3 = q[2];
		attitudeActual.q4 = q[3];
		AttitudeActualSet(&attitudeActual);
	}
	
	static float wind[3] = {0,0,0};
	wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
	wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
	wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
	
	Quaternion2R(q,Rbe);
	// Make thrust negative as down is positive
	ned_accel[0] = -thrust * Rbe[2][0];
	ned_accel[1] = -thrust * Rbe[2][1];
	// Gravity causes acceleration of 9.81 in the down direction
	ned_accel[2] = -thrust * Rbe[2][2] + GRAVITY;
	
	// Apply acceleration based on velocity
	ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
	ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
	ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);

	// Predict the velocity forward in time
	vel[0] = vel[0] + ned_accel[0] * dT;
	vel[1] = vel[1] + ned_accel[1] * dT;
	vel[2] = vel[2] + ned_accel[2] * dT;

	// Predict the position forward in time
	pos[0] = pos[0] + vel[0] * dT;
	pos[1] = pos[1] + vel[1] * dT;
	pos[2] = pos[2] + vel[2] * dT;

	// Simulate hitting ground
	if(pos[2] > 0) {
		pos[2] = 0;
		vel[2] = 0;
		ned_accel[2] = 0;
	}
		
	// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
	ned_accel[2] -= 9.81;
	
	// Transform the accels back in to body frame
	AccelsData accelsData; // Skip get as we set all the fields
	accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
	accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
	accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
	accelsData.temperature = 30;
	AccelsSet(&accelsData);

	if(baro_offset == 0) {
		// Hacky initialization
		baro_offset = 50;// * rand_gauss();
	} else {
		// Very small drift process
		baro_offset += rand_gauss() / 100;
	}
	// Update baro periodically	
	static uint32_t last_baro_time = 0;
	if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
		BaroAltitudeData baroAltitude;
		BaroAltitudeGet(&baroAltitude);
		baroAltitude.Altitude = -pos[2] + baro_offset;
		BaroAltitudeSet(&baroAltitude);
		last_baro_time = PIOS_DELAY_GetRaw();
	}
	
	HomeLocationData homeLocation;
	HomeLocationGet(&homeLocation);

	static float gps_vel_drift[3] = {0,0,0};
	gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
	gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
	gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;

	// Update GPS periodically	
	static uint32_t last_gps_time = 0;
	if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
		// Use double precision here as simulating what GPS produces
		double T[3];
		T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
		T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
		T[2] = -1.0;
		
		static float gps_drift[3] = {0,0,0};
		gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
		gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
		gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;

		GPSPositionData gpsPosition;
		GPSPositionGet(&gpsPosition);
		gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
		gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
		gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
		gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
		gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
		gpsPosition.Satellites = 7;
		gpsPosition.PDOP = 1;
		gpsPosition.Status = GPSPOSITION_STATUS_FIX3D;
		GPSPositionSet(&gpsPosition);
		last_gps_time = PIOS_DELAY_GetRaw();
	}
	
	// Update GPS Velocity measurements
	static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
	if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
		GPSVelocityData gpsVelocity;
		GPSVelocityGet(&gpsVelocity);
		gpsVelocity.North = vel[0] + gps_vel_drift[0];
		gpsVelocity.East = vel[1] + gps_vel_drift[1];
		gpsVelocity.Down = vel[2] + gps_vel_drift[2];
		GPSVelocitySet(&gpsVelocity);
		last_gps_vel_time = PIOS_DELAY_GetRaw();
	}

	// Update mag periodically
	static uint32_t last_mag_time = 0;
	if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
		MagnetometerData mag;
		mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
		mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
		mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];

		// Run the offset compensation algorithm from the firmware
		magOffsetEstimation(&mag);

		MagnetometerSet(&mag);
		last_mag_time = PIOS_DELAY_GetRaw();
	}
	
	AttitudeSimulatedData attitudeSimulated;
	AttitudeSimulatedGet(&attitudeSimulated);
	attitudeSimulated.q1 = q[0];
	attitudeSimulated.q2 = q[1];
	attitudeSimulated.q3 = q[2];
	attitudeSimulated.q4 = q[3];
	Quaternion2RPY(q,&attitudeSimulated.Roll);
	attitudeSimulated.Position[0] = pos[0];
	attitudeSimulated.Position[1] = pos[1];
	attitudeSimulated.Position[2] = pos[2];
	attitudeSimulated.Velocity[0] = vel[0];
	attitudeSimulated.Velocity[1] = vel[1];
	attitudeSimulated.Velocity[2] = vel[2];
	AttitudeSimulatedSet(&attitudeSimulated);
}
Exemplo n.º 11
0
/**
 * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
 * @params[in] first_run This is the first run so trigger reinitialization
 * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
 * @return 0 for success, -1 for failure
 */
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
	UAVObjEvent ev;
	GyrosData gyrosData;
	AccelsData accelsData;
	MagnetometerData magData;
	BaroAltitudeData baroData;
	GPSPositionData gpsData;
	GPSVelocityData gpsVelData;
	GyrosBiasData gyrosBias;

	static bool mag_updated = false;
	static bool baro_updated;
	static bool gps_updated;
	static bool gps_vel_updated;

	static float baroOffset = 0;

	static uint32_t ins_last_time = 0;
	static bool inited;

	float NED[3] = {0.0f, 0.0f, 0.0f};
	float vel[3] = {0.0f, 0.0f, 0.0f};
	float zeros[3] = {0.0f, 0.0f, 0.0f};

	// Perform the update
	uint16_t sensors = 0;
	float dT;

	// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
	if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
	     (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) )
	{
		// Do not set attitude timeout warnings in simulation mode
		if (!AttitudeActualReadOnly()){
			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
			return -1;
		}
	}

	if (inited) {
		mag_updated = 0;
		baro_updated = 0;
		gps_updated = 0;
		gps_vel_updated = 0;
	}

	if (first_run) {
		inited = false;
		init_stage = 0;

		mag_updated = 0;
		baro_updated = 0;
		gps_updated = 0;
		gps_vel_updated = 0;

		ins_last_time = PIOS_DELAY_GetRaw();

		return 0;
	}

	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
	gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;

	// Get most recent data
	GyrosGet(&gyrosData);
	AccelsGet(&accelsData);
	MagnetometerGet(&magData);
	BaroAltitudeGet(&baroData);
	GPSPositionGet(&gpsData);
	GPSVelocityGet(&gpsVelData);
	GyrosBiasGet(&gyrosBias);

	// Discard mag if it has NAN (normally from bad calibration)
	mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z);
	// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
	// switching between indoor and outdoor mode with Set = false)
	mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]);

	// Have a minimum requirement for gps usage
	gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);

	if (!inited)
		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
	else if (outdoor_mode && gpsData.Satellites < 7)
		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
	else
		AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
			
	if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
		// Don't initialize until all sensors are read
		if (init_stage == 0 && !outdoor_mode) {
			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
			float q[4];
			float pos[3] = {0.0f, 0.0f, 0.0f};

			// Initialize barometric offset to homelocation altitude
			baroOffset = -baroData.Altitude;
			pos[2] = -(baroData.Altitude + baroOffset);

			// Reset the INS algorithm
			INSGPSInit();
			INSSetMagVar(revoCalibration.mag_var);
			INSSetAccelVar(revoCalibration.accel_var);
			INSSetGyroVar(revoCalibration.gyro_var);
			INSSetBaroVar(revoCalibration.baro_var);

			// Initialize the gyro bias from the settings
			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
			INSSetGyroBias(gyro_bias);

			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
			MagnetometerGet(&magData);

			// Set initial attitude
			AttitudeActualData attitudeActual;
			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
			AttitudeActualSet(&attitudeActual);

			q[0] = attitudeActual.q1;
			q[1] = attitudeActual.q2;
			q[2] = attitudeActual.q3;
			q[3] = attitudeActual.q4;
			INSSetState(pos, zeros, q, zeros, zeros);
			INSResetP(Pdiag);
		} else if (init_stage == 0 && outdoor_mode) {
			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
			float q[4];
			float NED[3];

			// Reset the INS algorithm
			INSGPSInit();
			INSSetMagVar(revoCalibration.mag_var);
			INSSetAccelVar(revoCalibration.accel_var);
			INSSetGyroVar(revoCalibration.gyro_var);
			INSSetBaroVar(revoCalibration.baro_var);

			INSSetMagNorth(homeLocation.Be);

			// Initialize the gyro bias from the settings
			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
			INSSetGyroBias(gyro_bias);

			GPSPositionData gpsPosition;
			GPSPositionGet(&gpsPosition);

			// Transform the GPS position into NED coordinates
			getNED(&gpsPosition, NED);
			
			// Initialize barometric offset to cirrent GPS NED coordinate
			baroOffset = -NED[2] - baroData.Altitude;

			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
			MagnetometerGet(&magData);

			// Set initial attitude
			AttitudeActualData attitudeActual;
			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
			AttitudeActualSet(&attitudeActual);

			q[0] = attitudeActual.q1;
			q[1] = attitudeActual.q2;
			q[2] = attitudeActual.q3;
			q[3] = attitudeActual.q4;

			INSSetState(NED, zeros, q, zeros, zeros);
			INSResetP(Pdiag);
		} else if (init_stage > 0) {
			// Run prediction a bit before any corrections
			dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;

			GyrosBiasGet(&gyrosBias);
			float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
				(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
				(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
			INSStatePrediction(gyros, &accelsData.x, dT);
			
			AttitudeActualData attitude;
			AttitudeActualGet(&attitude);
			attitude.q1 = Nav.q[0];
			attitude.q2 = Nav.q[1];
			attitude.q3 = Nav.q[2];
			attitude.q4 = Nav.q[3];
			Quaternion2RPY(&attitude.q1,&attitude.Roll);
			AttitudeActualSet(&attitude);
		}

		init_stage++;
		if(init_stage > 10)
			inited = true;

		ins_last_time = PIOS_DELAY_GetRaw();	

		return 0;
	}

	if (!inited)
		return 0;

	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
	ins_last_time = PIOS_DELAY_GetRaw();

	// This should only happen at start up or at mode switches
	if(dT > 0.01f)
		dT = 0.01f;
	else if(dT <= 0.001f)
		dT = 0.001f;

	// If the gyro bias setting was updated we should reset
	// the state estimate of the EKF
	if(gyroBiasSettingsUpdated) {
		float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
		INSSetGyroBias(gyro_bias);
		gyroBiasSettingsUpdated = false;
	}

	// Because the sensor module remove the bias we need to add it
	// back in here so that the INS algorithm can track it correctly
	float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
		gyros[0] += gyrosBias.x * F_PI / 180.0f;
		gyros[1] += gyrosBias.y * F_PI / 180.0f;
		gyros[2] += gyrosBias.z * F_PI / 180.0f;
	}

	// Advance the state estimate
	INSStatePrediction(gyros, &accelsData.x, dT);

	// Copy the attitude into the UAVO
	AttitudeActualData attitude;
	AttitudeActualGet(&attitude);
	attitude.q1 = Nav.q[0];
	attitude.q2 = Nav.q[1];
	attitude.q3 = Nav.q[2];
	attitude.q4 = Nav.q[3];
	Quaternion2RPY(&attitude.q1,&attitude.Roll);
	AttitudeActualSet(&attitude);

	// Advance the covariance estimate
	INSCovariancePrediction(dT);

	if(mag_updated)
		sensors |= MAG_SENSORS;

	if(baro_updated)
		sensors |= BARO_SENSOR;

	INSSetMagNorth(homeLocation.Be);
	
	if (gps_updated && outdoor_mode)
	{
		INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
		sensors |= POS_SENSORS;

		if (0) { // Old code to take horizontal velocity from GPS Position update
			sensors |= HORIZ_SENSORS;
			vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
			vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
			vel[2] = 0;
		}
		// Transform the GPS position into NED coordinates
		getNED(&gpsData, NED);

		// Track barometric altitude offset with a low pass filter
		baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
		    (1.0f - BARO_OFFSET_LOWPASS_ALPHA )
		    * ( -NED[2] - baroData.Altitude );

	} else if (!outdoor_mode) {
		INSSetPosVelVar(1e2f, 1e2f);
		vel[0] = vel[1] = vel[2] = 0;
		NED[0] = NED[1] = 0;
		NED[2] = -(baroData.Altitude + baroOffset);
		sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
		sensors |= POS_SENSORS |VERT_SENSORS;
	}

	if (gps_vel_updated && outdoor_mode) {
		sensors |= HORIZ_SENSORS | VERT_SENSORS;
		vel[0] = gpsVelData.North;
		vel[1] = gpsVelData.East;
		vel[2] = gpsVelData.Down;
	}
	
	/*
	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
	 * although probably should occur within INS itself
	 */
	if (sensors)
		INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors);

	// Copy the position and velocity into the UAVO
	PositionActualData positionActual;
	PositionActualGet(&positionActual);
	positionActual.North = Nav.Pos[0];
	positionActual.East = Nav.Pos[1];
	positionActual.Down = Nav.Pos[2];
	PositionActualSet(&positionActual);
	
	VelocityActualData velocityActual;
	VelocityActualGet(&velocityActual);
	velocityActual.North = Nav.Vel[0];
	velocityActual.East = Nav.Vel[1];
	velocityActual.Down = Nav.Vel[2];
	VelocityActualSet(&velocityActual);

	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
		// Copy the gyro bias into the UAVO except when it was updated
		// from the settings during the calculation, then consume it
		// next cycle
		gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI;
		gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
		gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
		GyrosBiasSet(&gyrosBias);
	}

	return 0;
}
Exemplo n.º 12
0
static int32_t updateAttitudeComplementary(bool first_run)
{
	UAVObjEvent ev;
	GyrosData gyrosData;
	AccelsData accelsData;
	static int32_t timeval;
	float dT;
	static uint8_t init = 0;

	// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
	if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
	     xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
	{
		// When one of these is updated so should the other
		// Do not set attitude timeout warnings in simulation mode
		if (!AttitudeActualReadOnly()){
			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
			return -1;
		}
	}

	AccelsGet(&accelsData);

	// During initialization and 
	FlightStatusData flightStatus;
	FlightStatusGet(&flightStatus);
	if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
		// To initialize we need a valid mag reading
		if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
			return -1;
		MagnetometerData magData;
		MagnetometerGet(&magData);
#else
		MagnetometerData magData;
		magData.x = 100;
		magData.y = 0;
		magData.z = 0;
#endif
		AttitudeActualData attitudeActual;
		AttitudeActualGet(&attitudeActual);
		init = 0;
		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
		attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
		attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;

		RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
		AttitudeActualSet(&attitudeActual);

		timeval = PIOS_DELAY_GetRaw();

		return 0;

	}

	if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
		// For first 7 seconds use accels to get gyro bias
		attitudeSettings.AccelKp = 1;
		attitudeSettings.AccelKi = 0.9;
		attitudeSettings.YawBiasRate = 0.23;
		magKp = 1;
	} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
		attitudeSettings.AccelKp = 1;
		attitudeSettings.AccelKi = 0.9;
		attitudeSettings.YawBiasRate = 0.23;
		magKp = 1;
		init = 0;
	} else if (init == 0) {
		// Reload settings (all the rates)
		AttitudeSettingsGet(&attitudeSettings);
		magKp = 0.01f;
		init = 1;
	}

	GyrosGet(&gyrosData);

	// Compute the dT using the cpu clock
	dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
	timeval = PIOS_DELAY_GetRaw();

	float q[4];

	AttitudeActualData attitudeActual;
	AttitudeActualGet(&attitudeActual);

	float grot[3];
	float accel_err[3];

	// Get the current attitude estimate
	quat_copy(&attitudeActual.q1, q);

	// Rotate gravity to body frame and cross with accels
	grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
	grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
	grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
	CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);

	// Account for accel magnitude
	accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
	accel_mag = sqrtf(accel_mag);
	accel_err[0] /= accel_mag;
	accel_err[1] /= accel_mag;
	accel_err[2] /= accel_mag;	

	if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
	{
		// Rotate gravity to body frame and cross with accels
		float brot[3];
		float Rbe[3][3];
		MagnetometerData mag;
		
		Quaternion2R(q, Rbe);
		MagnetometerGet(&mag);

		// If the mag is producing bad data don't use it (normally bad calibration)
		if  (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
			rot_mult(Rbe, homeLocation.Be, brot);

			float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
			mag.x /= mag_len;
			mag.y /= mag_len;
			mag.z /= mag_len;

			float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
			brot[0] /= bmag;
			brot[1] /= bmag;
			brot[2] /= bmag;

			// Only compute if neither vector is null
			if (bmag < 1 || mag_len < 1)
				mag_err[0] = mag_err[1] = mag_err[2] = 0;
			else
				CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
		}
	} else {
		mag_err[0] = mag_err[1] = mag_err[2] = 0;
	}

	// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
	GyrosBiasData gyrosBias;
	GyrosBiasGet(&gyrosBias);
	gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
	gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
	gyrosBias.z -= mag_err[2] * magKi;
	GyrosBiasSet(&gyrosBias);

	// Correct rates based on error, integral component dealt with in updateSensors
	gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
	gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
	gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;

	// Work out time derivative from INSAlgo writeup
	// Also accounts for the fact that gyros are in deg/s
	float qdot[4];
	qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;

	// Take a time step
	q[0] = q[0] + qdot[0];
	q[1] = q[1] + qdot[1];
	q[2] = q[2] + qdot[2];
	q[3] = q[3] + qdot[3];

	if(q[0] < 0) {
		q[0] = -q[0];
		q[1] = -q[1];
		q[2] = -q[2];
		q[3] = -q[3];
	}

	// Renomalize
	qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	q[0] = q[0] / qmag;
	q[1] = q[1] / qmag;
	q[2] = q[2] / qmag;
	q[3] = q[3] / qmag;

	// If quaternion has become inappropriately short or is nan reinit.
	// THIS SHOULD NEVER ACTUALLY HAPPEN
	if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
		q[0] = 1;
		q[1] = 0;
		q[2] = 0;
		q[3] = 0;
	}

	quat_copy(q, &attitudeActual.q1);

	// Convert into eueler degrees (makes assumptions about RPY order)
	Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);

	AttitudeActualSet(&attitudeActual);

	// Flush these queues for avoid errors
	xQueueReceive(baroQueue, &ev, 0);
	if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
		float NED[3];
		// Transform the GPS position into NED coordinates
		GPSPositionData gpsPosition;
		GPSPositionGet(&gpsPosition);
		getNED(&gpsPosition, NED);
		
		PositionActualData positionActual;
		PositionActualGet(&positionActual);
		positionActual.North = NED[0];
		positionActual.East = NED[1];
		positionActual.Down = NED[2];
		PositionActualSet(&positionActual);
	}

	if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
		// Transform the GPS position into NED coordinates
		GPSVelocityData gpsVelocity;
		GPSVelocityGet(&gpsVelocity);

		VelocityActualData velocityActual;
		VelocityActualGet(&velocityActual);
		velocityActual.North = gpsVelocity.North;
		velocityActual.East = gpsVelocity.East;
		velocityActual.Down = gpsVelocity.Down;
		VelocityActualSet(&velocityActual);
	}


	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

	return 0;
}
Exemplo n.º 13
0
int main()
{
	float gyro[3], accel[3], mag[3];
	float vel[3] = { 0, 0, 0 };
	/* Normaly we get/set UAVObjects but this one only needs to be set.
	We will never expect to get this from another module*/
	AttitudeActualData attitude_actual;
	AHRSSettingsData ahrs_settings;

	/* Brings up System using CMSIS functions, enables the LEDs. */
	PIOS_SYS_Init();

	/* Delay system */
	PIOS_DELAY_Init();

	/* Communication system */
	PIOS_COM_Init();

	/* ADC system */
	AHRS_ADC_Config( adc_oversampling );

	/* Setup the Accelerometer FS (Full-Scale) GPIO */
	PIOS_GPIO_Enable( 0 );
	SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
	/* Magnetic sensor system */
	PIOS_I2C_Init();
	PIOS_HMC5843_Init();

	// Get 3 ID bytes
	strcpy(( char * )mag_data.id, "ZZZ" );
	PIOS_HMC5843_ReadID( mag_data.id );
#endif

	/* SPI link to master */
//	PIOS_SPI_Init();
	AhrsInitComms();
	AHRSCalibrationConnectCallback( calibration_callback );
	GPSPositionConnectCallback( gps_callback );

	ahrs_state = AHRS_IDLE;

	while( !AhrsLinkReady() ) {
		AhrsPoll();
		while( ahrs_state != AHRS_DATA_READY ) ;
		ahrs_state = AHRS_PROCESSING;
		downsample_data();
		ahrs_state = AHRS_IDLE;
		if(( total_conversion_blocks % 50 ) == 0 )
			PIOS_LED_Toggle( LED1 );
	}


	AHRSSettingsGet(&ahrs_settings);


	/* Use simple averaging filter for now */
	for( int i = 0; i < adc_oversampling; i++ )
		fir_coeffs[i] = 1;
	fir_coeffs[adc_oversampling] = adc_oversampling;

	if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_INSGPS) {
		// compute a data point and initialize INS
		downsample_data();
		converge_insgps();
	}


#ifdef DUMP_RAW
	int previous_conversion;
	while( 1 ) {
		AhrsPoll();
		int result;
		uint8_t framing[16] = {
			0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
			15
		};
		while( ahrs_state != AHRS_DATA_READY ) ;
		ahrs_state = AHRS_PROCESSING;

		if( total_conversion_blocks != previous_conversion + 1 )
			PIOS_LED_On( LED1 );	// not keeping up
		else
			PIOS_LED_Off( LED1 );
		previous_conversion = total_conversion_blocks;

		downsample_data();
		ahrs_state = AHRS_IDLE;;

		// Dump raw buffer
		result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 );	// framing header
		result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) );	// dump block number
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & valid_data_buffer[0],
								 ADC_OVERSAMPLE *
								 ADC_CONTINUOUS_CHANNELS *
								 sizeof( valid_data_buffer[0] ) );
		if( result == 0 )
			PIOS_LED_Off( LED1 );
		else {
			PIOS_LED_On( LED1 );
		}
	}
#endif

	timer_start();

	/******************* Main EKF loop ****************************/
	while( 1 ) {
		AhrsPoll();
		AHRSCalibrationData calibration;
		AHRSCalibrationGet( &calibration );
		BaroAltitudeData baro_altitude;
		BaroAltitudeGet( &baro_altitude );
		GPSPositionData gps_position;
		GPSPositionGet( &gps_position );
		AHRSSettingsGet(&ahrs_settings);

		// Alive signal
		if(( total_conversion_blocks % 100 ) == 0 )
			PIOS_LED_Toggle( LED1 );

#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		// Get magnetic readings
		if( PIOS_HMC5843_NewDataAvailable() ) {
			PIOS_HMC5843_ReadMag( mag_data.raw.axis );
			mag_data.updated = 1;
		}
		attitude_raw.magnetometers[0] = mag_data.raw.axis[0];
		attitude_raw.magnetometers[2] = mag_data.raw.axis[1];
		attitude_raw.magnetometers[2] = mag_data.raw.axis[2];

#endif
		// Delay for valid data

		counter_val = timer_count();
		running_counts = counter_val - last_counter_idle_end;
		last_counter_idle_start = counter_val;

		while( ahrs_state != AHRS_DATA_READY ) ;

		counter_val = timer_count();
		idle_counts = counter_val - last_counter_idle_start;
		last_counter_idle_end = counter_val;

		ahrs_state = AHRS_PROCESSING;

		downsample_data();

		/***************** SEND BACK SOME RAW DATA ************************/
		// Hacky - grab one sample from buffer to populate this.  Need to send back
		// all raw data if it's happening
		accel_data.raw.x = valid_data_buffer[0];
		accel_data.raw.y = valid_data_buffer[2];
		accel_data.raw.z = valid_data_buffer[4];

		gyro_data.raw.x = valid_data_buffer[1];
		gyro_data.raw.y = valid_data_buffer[3];
		gyro_data.raw.z = valid_data_buffer[5];

		gyro_data.temp.xy = valid_data_buffer[6];
		gyro_data.temp.z = valid_data_buffer[7];

		if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_INSGPS) {
			/******************** INS ALGORITHM **************************/

			// format data for INS algo
			gyro[0] = gyro_data.filtered.x;
			gyro[1] = gyro_data.filtered.y;
			gyro[2] = gyro_data.filtered.z;
			accel[0] = accel_data.filtered.x,
					   accel[1] = accel_data.filtered.y,
								  accel[2] = accel_data.filtered.z,
											 // Note: The magnetometer driver returns registers X,Y,Z from the chip which are
											 // (left, backward, up).  Remapping to (forward, right, down).
											 mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] );
			mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] );
			mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] );

			INSStatePrediction( gyro, accel,
								1 / ( float )EKF_RATE );
			INSCovariancePrediction( 1 / ( float )EKF_RATE );

			if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) {
				// Compute velocity from Heading and groundspeed
				vel[0] =
					gps_position.Groundspeed *
					cos( gps_position.Heading * M_PI / 180 );
				vel[1] =
					gps_position.Groundspeed *
					sin( gps_position.Heading * M_PI / 180 );

				// Completely unprincipled way to make the position variance
				// increase as data quality decreases but keep it bounded
				// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
				INSSetPosVelVar( 0.004 );

				HomeLocationData home;
				HomeLocationGet( &home );
				float ned[3];
				double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )};
				// convert from cm back to meters
				double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )};
				LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned );

				if( gps_updated ) { //FIXME: Is this correct?
					//TOOD: add check for altitude updates
					FullCorrection( mag, ned,
									vel,
									baro_altitude.Altitude );
					gps_updated = false;
				} else {
					GpsBaroCorrection( ned,
									   vel,
									   baro_altitude.Altitude );
				}

				gps_updated = false;
				mag_data.updated = 0;
			} else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D
					   && mag_data.updated == 1 ) {
				MagCorrection( mag );	// only trust mags if outdoors
				mag_data.updated = 0;
			} else {
				// Indoors, update with zero position and velocity and high covariance
				INSSetPosVelVar( 0.1 );
				vel[0] = 0;
				vel[1] = 0;
				vel[2] = 0;

				VelBaroCorrection( vel,
								   baro_altitude.Altitude );
//                MagVelBaroCorrection(mag,vel,altitude_data.altitude);  // only trust mags if outdoors
			}

			attitude_actual.q1 = Nav.q[0];
			attitude_actual.q2 = Nav.q[1];
			attitude_actual.q3 = Nav.q[2];
			attitude_actual.q4 = Nav.q[3];
		} else if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_SIMPLE ) {
			float q[4];
			float rpy[3];
			/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
			/* Very simple computation of the heading and attitude from accel. */
			rpy[2] =
				atan2(( mag_data.raw.axis[0] ),
					  ( -1 * mag_data.raw.axis[1] ) ) * 180 /
				M_PI;
			rpy[1] =
				atan2( accel_data.filtered.x,
					   accel_data.filtered.z ) * 180 / M_PI;
			rpy[0] =
				atan2( accel_data.filtered.y,
					   accel_data.filtered.z ) * 180 / M_PI;

			RPY2Quaternion( rpy, q );
			attitude_actual.q1 = q[0];
			attitude_actual.q2 = q[1];
			attitude_actual.q3 = q[2];
			attitude_actual.q4 = q[3];
		}

		ahrs_state = AHRS_IDLE;

#ifdef DUMP_FRIENDLY
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n",
				total_conversion_blocks );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n",
				( int16_t )( accel_data.filtered.x * 1000 ),
				( int16_t )( accel_data.filtered.y * 1000 ),
				( int16_t )( accel_data.filtered.z * 1000 ) );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n",
				( int16_t )( gyro_data.filtered.x * 1000 ),
				( int16_t )( gyro_data.filtered.y * 1000 ),
				( int16_t )( gyro_data.filtered.z * 1000 ) );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n",
				mag_data.raw.axis[0],
				mag_data.raw.axis[1],
				mag_data.raw.axis[2] );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX,
				"q: %d %d %d %d\r\n",
				( int16_t )( Nav.q[0] * 1000 ),
				( int16_t )( Nav.q[1] * 1000 ),
				( int16_t )( Nav.q[2] * 1000 ),
				( int16_t )( Nav.q[3] * 1000 ) );
#endif
#ifdef DUMP_EKF
		uint8_t framing[16] = {
			15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
			0
		};
		extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
		extern float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
		extern float Q[NUMW], R[NUMV];	// input noise and measurement noise variances
		extern float K[NUMX][NUMV];	// feedback gain matrix

		// Dump raw buffer
		int8_t result;
		result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 );	// framing header
		result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) );	// dump block number
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & mag_data,
								 sizeof( mag_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & gps_data,
								 sizeof( gps_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & accel_data,
								 sizeof( accel_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & gyro_data,
								 sizeof( gyro_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q,
								 sizeof( float ) * NUMX * NUMX );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K,
								 sizeof( float ) * NUMX * NUMV );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X,
								 sizeof( float ) * NUMX * NUMX );

		if( result == 0 )
			PIOS_LED_Off( LED1 );
		else {
			PIOS_LED_On( LED1 );
		}
#endif
		AttitudeActualSet( &attitude_actual );

		/*FIXME: This is dangerous. There is no locking for UAVObjects
		so it could stomp all over the airspeed/climb rate etc.
		This used to be done in the OP module which was bad.
		Having ~4ms latency for the round trip makes it worse here.
		*/
		PositionActualData pos;
		PositionActualGet( &pos );
		for( int ct = 0; ct < 3; ct++ ) {
			pos.NED[ct] = Nav.Pos[ct];
			pos.Vel[ct] = Nav.Vel[ct];
		}
		PositionActualSet( &pos );

		static bool was_calibration = false;
		AhrsStatusData status;
		AhrsStatusGet( &status );
		if( was_calibration != status.CalibrationSet ) {
			was_calibration = status.CalibrationSet;
			if( status.CalibrationSet ) {
				calibrate_sensors();
				AhrsStatusGet( &status );
				status.CalibrationSet = true;
			}
		}
		status.CPULoad = (( float )running_counts /
						  ( float )( idle_counts + running_counts ) ) * 100;

		status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 );
		status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 );
		status.DroppedUpdates = ekf_too_slow;
		AhrsStatusSet( &status );

	}

	return 0;
}
Exemplo n.º 14
0
/**
 * Telemetry transmit task. Processes queue events and periodic updates.
 */
static void telemetryRxTask(void *parameters)
{
	uint32_t inputPort;
	uint8_t	c;

	// Task loop
	while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
		// Determine input port (USB takes priority over telemetry port)
		if (PIOS_USB_HID_CheckAvailable(0)) {
			inputPort = PIOS_COM_TELEM_USB;
		} else
#endif /* PIOS_INCLUDE_USB_HID */
		{
			inputPort = telemetryPort;
		}

		mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;

		// Block until a byte is available
		PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);

		// And process it

		if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
		{

			// Handle packet with waypoint component
			mavlink_wpm_message_handler(&rx_msg);

			// Handle packet with parameter component
			mavlink_pm_message_handler(mavlink_chan, &rx_msg);

			switch (rx_msg.msgid)
			{
			case MAVLINK_MSG_ID_HEARTBEAT:
			{
				// Check if this is the gcs
				mavlink_heartbeat_t beat;
				mavlink_msg_heartbeat_decode(&rx_msg, &beat);
				if (beat.type == MAV_TYPE_GCS)
				{
					// Got heartbeat from the GCS, we're good!
					lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
				}
			}
			break;
			case MAVLINK_MSG_ID_SET_MODE:
			{
				mavlink_set_mode_t mode;
				mavlink_msg_set_mode_decode(&rx_msg, &mode);
				// Check if this system should change the mode
				if (mode.target_system == mavlink_system.sysid)
				{
					FlightStatusData flightStatus;
					FlightStatusGet(&flightStatus);

					switch (mode.base_mode)
					{
					case MAV_MODE_MANUAL_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_MANUAL_DISARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
					}
					break;
					case MAV_MODE_PREFLIGHT:
					{
						flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
					}
					break;
					case MAV_MODE_STABILIZE_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_GUIDED_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_AUTO_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					}

					bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
					if (newHilEnabled != hilEnabled)
					{
						if (newHilEnabled)
						{
							// READ-ONLY flag write to ActuatorCommand
							UAVObjMetadata meta;
							UAVObjHandle handle = ActuatorCommandHandle();
							UAVObjGetMetadata(handle, &meta);
							meta.access = ACCESS_READONLY;
							UAVObjSetMetadata(handle, &meta);

							mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
							mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
							mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
						}
						else
						{
							// READ-ONLY flag write to ActuatorCommand
							UAVObjMetadata meta;
							UAVObjHandle handle = ActuatorCommandHandle();
							UAVObjGetMetadata(handle, &meta);
							meta.access = ACCESS_READWRITE;
							UAVObjSetMetadata(handle, &meta);

							mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
							mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
							mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
						}
					}
					hilEnabled = newHilEnabled;

					FlightStatusSet(&flightStatus);

					// Check HIL
					bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
					enableHil(hilEnabled);
				}
			}
			break;
			case MAVLINK_MSG_ID_HIL_STATE:
			{
				if (hilEnabled)
				{
					mavlink_hil_state_t hil;
					mavlink_msg_hil_state_decode(&rx_msg, &hil);

					// Write GPSPosition
					GPSPositionData gps;
					GPSPositionGet(&gps);
					gps.Altitude = hil.alt/10;
					gps.Latitude = hil.lat/10;
					gps.Longitude = hil.lon/10;
					GPSPositionSet(&gps);

					// Write PositionActual
					PositionActualData pos;
					PositionActualGet(&pos);
					// FIXME WRITE POSITION HERE
					PositionActualSet(&pos);

					// Write AttitudeActual
					AttitudeActualData att;
					AttitudeActualGet(&att);
					att.Roll = hil.roll;
					att.Pitch = hil.pitch;
					att.Yaw = hil.yaw;
					// FIXME
					//att.RollSpeed = hil.rollspeed;
					//att.PitchSpeed = hil.pitchspeed;
					//att.YawSpeed = hil.yawspeed;

					// Convert to quaternion formulation
					RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
					// Write AttitudeActual
					AttitudeActualSet(&att);

					// Write AttitudeRaw
					AttitudeRawData raw;
					AttitudeRawGet(&raw);
					raw.gyros[0] = hil.rollspeed;
					raw.gyros[1] = hil.pitchspeed;
					raw.gyros[2] = hil.yawspeed;
					raw.accels[0] = hil.xacc;
					raw.accels[1] = hil.yacc;
					raw.accels[2] = hil.zacc;
					//				raw.magnetometers[0] = hil.xmag;
					//				raw.magnetometers[0] = hil.ymag;
					//				raw.magnetometers[0] = hil.zmag;
					AttitudeRawSet(&raw);
				}
			}
			break;
			case MAVLINK_MSG_ID_COMMAND_LONG:
			{
				// FIXME Implement
			}
			break;
			}
		}
	}
}
Exemplo n.º 15
0
/**
 * Processes queue events
 */
static void processObjEvent(UAVObjEvent * ev)
{
	UAVObjMetadata metadata;
	//	FlightTelemetryStatsData flightStats;
	//	GCSTelemetryStatsData gcsTelemetryStatsData;
	//	int32_t retries;
	//	int32_t success;

	if (ev->obj == 0) {
		updateTelemetryStats();
	} else if (ev->obj == GCSTelemetryStatsHandle()) {
		gcsTelemetryStatsUpdated();
	} else if (ev->obj == TelemetrySettingsHandle()) {
		updateSettings();
	} else {

		// Get object metadata
		UAVObjGetMetadata(ev->obj, &metadata);

		// If this is a metaobject then make necessary telemetry updates
		if (UAVObjIsMetaobject(ev->obj)) {
			updateObject(UAVObjGetLinkedObj(ev->obj));	// linked object will be the actual object the metadata are for
		}

		mavlink_message_t msg;

		mavlink_system.sysid = 20;
		mavlink_system.compid = MAV_COMP_ID_IMU;
		mavlink_system.type = MAV_TYPE_FIXED_WING;
		uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT;

		AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);

		// Setup type and object id fields
		uint32_t objId = UAVObjGetID(ev->obj);

		//		uint64_t timeStamp = 0;
		switch(objId) {
		case BAROALTITUDE_OBJID:
		{
			BaroAltitudeGet(&baroAltitude);
			pressure.press_abs = baroAltitude.Pressure*10.0f;
			pressure.temperature = baroAltitude.Temperature*100.0f;
			mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure);
			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
			break;
		}
		case FLIGHTTELEMETRYSTATS_OBJID:
		{
			//				FlightTelemetryStatsData flightTelemetryStats;
			FlightTelemetryStatsGet(&flightStats);

			// XXX this is a hack to make it think it got a confirmed
			// connection
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
			GCSTelemetryStatsGet(&gcsTelemetryStatsData);
			gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
			//
			//
			//				//mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass);
			//				mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass);
			//				// Copy the message to the send buffer
			//				uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			//				// Send buffer
			//				PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
			break;
		}
		case SYSTEMSTATS_OBJID:
		{
			FlightStatusData flightStatus;
			FlightStatusGet(&flightStatus);

			uint8_t system_state = MAV_STATE_UNINIT;
			uint8_t base_mode = 0;
			uint8_t custom_mode = 0;

			// Set flight mode
			switch (flightStatus.FlightMode)
			{
			case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
				base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
				break;
			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
				base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
				break;
			case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
				base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
				break;
			case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
				base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
				break;
			case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
				base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
				break;
			case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
				base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
				break;
			default:
				base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
				break;
			}

			// Set arming state
			switch (flightStatus.Armed)
			{
			case FLIGHTSTATUS_ARMED_ARMING:
			case FLIGHTSTATUS_ARMED_ARMED:
				system_state = MAV_STATE_ACTIVE;
				base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
				break;
			case FLIGHTSTATUS_ARMED_DISARMED:
				system_state = MAV_STATE_STANDBY;
				base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED;
				break;
			}

			// Set HIL
			if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;

			mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state);

			SystemStatsData stats;
			SystemStatsGet(&stats);
			FlightBatteryStateData flightBatteryData;
			FlightBatteryStateGet(&flightBatteryData);
			FlightBatterySettingsData flightBatterySettings;
			FlightBatterySettingsGet(&flightBatterySettings);

			uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f);
			int16_t batteryCurrent = -1; // -1: Not present / not estimated
			int8_t batteryPercent = -1; // -1: Not present / not estimated
			//			if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0)
			//			{
			// Factor is zero, sensor is not present
			// Estimate remaining capacity based on lipo curve
			batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f));
			//			}
			//			else
			//			{
			//				// Use capacity and current
			//				batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity);
			//				batteryCurrent = flightBatteryData.Current*100;
			//			}

				mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0);
//				// Copy the message to the send buffer
//				uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
//				// Send buffer
//				PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
			break;
		}
		case ATTITUDERAW_OBJID:
		{
			AttitudeRawGet(&attitudeRaw);

			// Copy data
			attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X];
			attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y];
			attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z];
			attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X];
			attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y];
			attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z];
			attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X];
			attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y];
			attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z];

			mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw);
			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);

			if (hilEnabled)
			{
				mavlink_hil_controls_t controls;

				// Copy data
				controls.roll_ailerons = 0.1;
				controls.pitch_elevator = 0.1;
				controls.yaw_rudder = 0.0;
				controls.throttle = 0.8;

				mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls);
				// Copy the message to the send buffer
				len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
				// Send buffer
				PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
			}
			break;
		}
		case ATTITUDEMATRIX_OBJID:
		{
			AttitudeMatrixGet(&attitudeMatrix);

			// Copy data
			attitude.roll = attitudeMatrix.Roll;
			attitude.pitch = attitudeMatrix.Pitch;
			attitude.yaw = attitudeMatrix.Yaw;

			attitude.rollspeed = attitudeMatrix.AngularRates[0];
			attitude.pitchspeed = attitudeMatrix.AngularRates[1];
			attitude.yawspeed = attitudeMatrix.AngularRates[2];

			mavlink_msg_attitude_encode(mavlink_system.sysid,
					mavlink_system.compid, &msg, &attitude);
			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
			break;
		}
		case GPSPOSITION_OBJID:
		{
			GPSPositionGet(&gpsPosition);
			gps_raw.time_usec = 0;
			gps_raw.lat = gpsPosition.Latitude*10;
			gps_raw.lon = gpsPosition.Longitude*10;
			gps_raw.alt = gpsPosition.Altitude*10;
			gps_raw.eph = gpsPosition.HDOP*100;
			gps_raw.epv = gpsPosition.VDOP*100;
			gps_raw.cog = gpsPosition.Heading*100;
			gps_raw.satellites_visible = gpsPosition.Satellites;
			gps_raw.fix_type = gpsPosition.Status;
			mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw);
			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);

			//			mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0);

			break;
		}
		case POSITIONACTUAL_OBJID:
		{
			PositionActualData pos;
			PositionActualGet(&pos);
			mavlink_local_position_ned_t m_pos;
			m_pos.time_boot_ms = 0;
			m_pos.x = pos.North;
			m_pos.y = pos.East;
			m_pos.z = pos.Down;
			m_pos.vx = 0.0f;
			m_pos.vy = 0.0f;
			m_pos.vz = 0.0f;

			mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos);

			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
		}
		break;
		case ACTUATORCOMMAND_OBJID:
		{
			mavlink_rc_channels_scaled_t rc;
			float val;
			ManualControlCommandRollGet(&val);
			rc.chan1_scaled = val*1000;
			ManualControlCommandPitchGet(&val);
			rc.chan2_scaled = val*1000;
			ManualControlCommandYawGet(&val);
			rc.chan3_scaled = val*1000;
			ManualControlCommandThrottleGet(&val);
			rc.chan4_scaled = val*1000;

			ActuatorCommandData act;
			ActuatorCommandGet(&act);

			rc.chan5_scaled = act.Channel[0];
			rc.chan6_scaled = act.Channel[1];
			rc.chan7_scaled = act.Channel[2];
			rc.chan8_scaled = act.Channel[3];

			ManualControlCommandData cmd;
			ManualControlCommandGet(&cmd);

			rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
			rc.port = 0;

			mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);


			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
			break;
		}
		case MANUALCONTROLCOMMAND_OBJID:
		{
			mavlink_rc_channels_scaled_t rc;
			float val;
			ManualControlCommandRollGet(&val);
			rc.chan1_scaled = val*1000;
			ManualControlCommandPitchGet(&val);
			rc.chan2_scaled = val*1000;
			ManualControlCommandYawGet(&val);
			rc.chan3_scaled = val*1000;
			ManualControlCommandThrottleGet(&val);
			rc.chan4_scaled = val*1000;

			rc.chan5_scaled = 0;
			rc.chan6_scaled = 0;
			rc.chan7_scaled = 0;
			rc.chan8_scaled = 0;

			ManualControlCommandData cmd;
			ManualControlCommandGet(&cmd);

			rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
			rc.port = 0;

			mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);


			// Copy the message to the send buffer
			uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
			// Send buffer
			PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
			break;
		}
		default:
		{
			//printf("unknown object: %x\n",(unsigned int)objId);
			break;
		}
		}
	}
}