Esempio n. 1
0
void MAVUDBExtraOutput_40hz(void)
{
	// SEND SERIAL_UDB_EXTRA (SUE) VIA MAVLINK FOR BACKWARDS COMPATIBILITY with FLAN.PYW (FLIGHT ANALYZER)
	// The MAVLink messages for this section of code are unique to MatrixPilot and are defined in matrixpilot.xml
//	spread_transmission_load = 10;
//	if (mavlink_frequency_send(streamRates[MAV_DATA_STREAM_EXTRA1], mavlink_counter_40hz + spread_transmission_load)) // SUE code historically ran at 8HZ
	{
		switch (mavlink_sue_telemetry_counter)
		{
			case 8:
				mavlink_msg_serial_udb_extra_f14_send(MAVLINK_COMM_0, WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE,
				    get_reset_flags(), trap_flags, trap_source, osc_fail_count, CLOCK_CONFIG, FLIGHT_PLAN_TYPE);
				mavlink_sue_telemetry_counter--;
				break;
			case 7:
				mavlink_msg_serial_udb_extra_f15_send(MAVLINK_COMM_0, (uint8_t*)ID_VEHICLE_MODEL_NAME, (uint8_t*)ID_VEHICLE_REGISTRATION);
				mavlink_sue_telemetry_counter--;
				break;
			case 6:
				mavlink_msg_serial_udb_extra_f16_send(MAVLINK_COMM_0, (uint8_t*)ID_LEAD_PILOT, (uint8_t*)ID_DIY_DRONES_URL);
				mavlink_sue_telemetry_counter--;
				break;
			case 5:
				mavlink_msg_serial_udb_extra_f4_send(MAVLINK_COMM_0, ROLL_STABILIZATION_AILERONS, ROLL_STABILIZATION_RUDDER, PITCH_STABILIZATION,
				    YAW_STABILIZATION_RUDDER, YAW_STABILIZATION_AILERON, AILERON_NAVIGATION, RUDDER_NAVIGATION, ALTITUDEHOLD_STABILIZED,
				    ALTITUDEHOLD_WAYPOINT, RACING_MODE);
				mavlink_sue_telemetry_counter--;
				break;
			case 4:
				mavlink_msg_serial_udb_extra_f5_send(MAVLINK_COMM_0, YAWKP_AILERON, YAWKD_AILERON, ROLLKP, ROLLKD,
				    YAW_STABILIZATION_AILERON, AILERON_BOOST);
				mavlink_sue_telemetry_counter--;
				break;
			case 3:
				mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_0, PITCHGAIN, PITCHKD, RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST);
				mavlink_sue_telemetry_counter--;
				break;
			case 2:
				mavlink_msg_serial_udb_extra_f7_send(MAVLINK_COMM_0, YAWKP_RUDDER, YAWKD_RUDDER, ROLLKP_RUDDER, ROLLKD_RUDDER,
				    RUDDER_BOOST, RTL_PITCH_DOWN);
				mavlink_sue_telemetry_counter--;
				break;
			case 1:
				mavlink_msg_serial_udb_extra_f8_send(MAVLINK_COMM_0, HEIGHT_TARGET_MAX, HEIGHT_TARGET_MIN, ALT_HOLD_THROTTLE_MIN,
				    ALT_HOLD_THROTTLE_MAX, ALT_HOLD_PITCH_MIN, ALT_HOLD_PITCH_MAX, ALT_HOLD_PITCH_HIGH);
				mavlink_sue_telemetry_counter--;
				break;
			default:
			{
				if (mavlink_sue_telemetry_f2_a == true)
				{
					int16_t i;

					mavlink_sue_telemetry_f2_a = false;
					// Approximate time passing between each telemetry line, even though
					// we may not have new GPS time data each time through.
					// This line is important when GPS lock is lost during flight
					// It allows telemetry to have a time reference when the GPS time reference is lost
					// Note this does increment the official Time of Week (TOW) for the entire system.
					// It is not changed for now, to preserve close compatibility with origin SERIAL_UDB_EXTRA code.
					if (tow.WW > 0) tow.WW += 250;

					if (flags._.f13_print_req == 1)
					{
						// The F13 line of telemetry is printed just once  when origin has been captured after GPS lock
						mavlink_msg_serial_udb_extra_f13_send(MAVLINK_COMM_0, week_no.BB, lat_origin.WW, lon_origin.WW, alt_origin.WW);
						flags._.f13_print_req = 0;
					}
#if (MAG_YAW_DRIFT == 1)
					mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_0, tow.WW,
					    ((udb_flags._.radio_on << 2) + (dcm_flags._.nav_capable << 1) + flags._.GPS_steering),
					    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
					    rmat[0], rmat[1], rmat[2], rmat[3], rmat[4], rmat[5], rmat[6], rmat[7], rmat[8],
					    (uint16_t) cog_gps.BB, sog_gps.BB, (uint16_t) udb_cpu_load(), voltage_milis.BB,
					    air_speed_3DIMU, estimatedWind[0], estimatedWind[1], estimatedWind[2],
					    magFieldEarth[0], magFieldEarth[1], magFieldEarth[2],
					    svs, hdop);
#else
					mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_0, tow.WW,
					    ((udb_flags._.radio_on << 2) + (dcm_flags._.nav_capable << 1) + flags._.GPS_steering),
					    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
					    rmat[0], rmat[1], rmat[2], rmat[3], rmat[4], rmat[5], rmat[6], rmat[7], rmat[8],
					    (uint16_t) cog_gps.BB, sog_gps.BB, (uint16_t) udb_cpu_load(), voltage_milis.BB,
					    air_speed_3DIMU, estimatedWind[0], estimatedWind[1], estimatedWind[2],
					    0, 0, 0,
					    svs, hdop);
#endif // (MAG_YAW_DRIFT == 1)
					// Save  pwIn and PwOut buffers for sending next time around in f2_b format message
					for (i = 0; i <= (NUM_INPUTS > MAVLINK_SUE_CHANNEL_MAX_SIZE ? MAVLINK_SUE_CHANNEL_MAX_SIZE : NUM_INPUTS); i++)
						pwIn_save[i] = udb_pwIn[i];
					for (i = 0; i <= (NUM_OUTPUTS > MAVLINK_SUE_CHANNEL_MAX_SIZE ? MAVLINK_SUE_CHANNEL_MAX_SIZE : NUM_OUTPUTS); i++)
						pwOut_save[i] = udb_pwOut[i];
					}
					else
					{
						int16_t stack_free = 0;
						mavlink_sue_telemetry_f2_a = true;
#if (RECORD_FREE_STACK_SPACE == 1)
						stack_free = (int16_t)(4096-maxstack); // This is actually wrong for the UDB4, but currently left the same as for telemetry.c
#endif // (RECORD_FREE_STACK_SPACE == 1)

						mavlink_msg_serial_udb_extra_f2_b_send(MAVLINK_COMM_0, tow.WW,
						    pwIn_save[1], pwIn_save[2], pwIn_save[3], pwIn_save[4], pwIn_save[5],
						    pwIn_save[6], pwIn_save[7], pwIn_save[8], pwIn_save[9], pwIn_save[10],
						    pwOut_save[1], pwOut_save[2], pwOut_save[3], pwOut_save[4], pwOut_save[5],
						    pwOut_save[6], pwOut_save[7], pwOut_save[8], pwOut_save[9], pwOut_save[10],
						    IMUlocationx._.W1, IMUlocationy._.W1, IMUlocationz._.W1, flags.WW,
#if (SILSIM != 1)
						    osc_fail_count,
#else
						    0,
#endif // (SILSIM != 1)
						    IMUvelocityx._.W1, IMUvelocityy._.W1, IMUvelocityz._.W1,
						    goal.x, goal.y, goal.height, stack_free);
					}
				}
		}
	}
}
Esempio n. 2
0
void osd_update_values( void )
{
	switch (osd_phase)
	{
		case 0:
		{
#if (OSD_LOC_ALTITUDE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_ALTITUDE+1) ;
			osd_spi_write_number(IMUlocationz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ;		// Altitude
#endif
			
#if (OSD_LOC_CPU_LOAD != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_CPU_LOAD+1) ;
			osd_spi_write_number(udb_cpu_load(), 3, 0, 0, 0, 0) ;					// CPU
#endif
			
#if (OSD_LOC_VARIO_NUM != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_VARIO_NUM) ;
			osd_spi_write_number(IMUvelocityz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ;		// Variometer
#endif
			
#if (OSD_LOC_VARIO_ARROW != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_VARIO_ARROW) ;
			if (IMUvelocityz._.W1 <= -VARIOMETER_HIGH)
				osd_spi_write(0x7, 0xD4) ;						// Variometer down fast
			else if (IMUvelocityz._.W1 <= -VARIOMETER_LOW)
				osd_spi_write(0x7, 0xD2) ;						// Variometer down slowly
			else if (IMUvelocityz._.W1 < VARIOMETER_LOW)
				osd_spi_write(0x7, 0x00) ;						// Variometer flat (was 0xD0)
			else if (IMUvelocityz._.W1 < VARIOMETER_HIGH)
				osd_spi_write(0x7, 0xD1) ;						// Variometer up slowly
			else if (IMUvelocityz._.W1 >= VARIOMETER_HIGH)
				osd_spi_write(0x7, 0xD3) ;						// Variometer up fast
#endif
			
#if (OSD_LOC_AP_MODE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_AP_MODE) ;
			if (!flags._.pitch_feedback)
				osd_spi_write(0x7, 0x97) ;						// M : Manual Mode
			else if (!flags._.GPS_steering)
				osd_spi_write(0x7, 0x9D) ;						// S : Stabilized Mode
			else if (udb_flags._.radio_on && !flags._.rtl_hold)
				osd_spi_write(0x7, 0xA1) ;						// W : Waypoint Mode
			else if (flags._.rtl_hold && udb_flags._.radio_on)
				osd_spi_write(0x7, 0x92) ;						// H : RTL Hold, has signal
			else
				osd_spi_write(0x7, 0x9C) ;						// R : RTL Mode, lost signal
#endif
			break ;
		}
		case 1:
		{
			signed char dir_to_goal ;
			int dist_to_goal ;
			
			struct relative2D curHeading ;
			curHeading.x = -rmat[1] ;
			curHeading.y = rmat[4] ;
			signed char earth_yaw = rect_to_polar(&curHeading) ;// 0-255 (0=East,  ccw)
			
			if (flags._.GPS_steering)
			{
				dir_to_goal = desired_dir - earth_yaw ;
				dist_to_goal = abs(tofinish_line) ;
			}
			else 
			{
				struct relative2D toGoal ;
				toGoal.x = 0 - IMUlocationx._.W1 ;
				toGoal.y = 0 - IMUlocationy._.W1 ;
				dir_to_goal = rect_to_polar ( &toGoal ) - earth_yaw ;
				dist_to_goal = toGoal.x ;
			}
	
#if (OSD_LOC_DIST_TO_GOAL != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_DIST_TO_GOAL+1) ;
			osd_spi_write_number(dist_to_goal, 0, 0, 0, 0, 0) ;	// Distance to wp/home
#endif
			
#if (OSD_LOC_ARROW_TO_GOAL != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_ARROW_TO_GOAL) ;
			osd_write_arrow(dir_to_goal) ;
#endif
			
#if (OSD_LOC_HEADING_NUM != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_HEADING_NUM+1) ;
			// earth_yaw										// 0-255 (0=East,  ccw)
			int angle = (earth_yaw * 180 + 64) >> 7 ;			// 0-359 (0=East,  ccw)
			angle = -angle + 90;								// 0-359 (0=North, clockwise)
			if (angle < 0) angle += 360 ;						// 0-359 (0=North, clockwise)
			osd_spi_write_number(angle, 3, 0, NUM_FLAG_ZERO_PADDED, 0, 0) ;	// heading
#endif
			
#if (OSD_LOC_HEADING_CARDINAL != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_HEADING_CARDINAL) ;
			osd_spi_write_string(heading_strings[((unsigned char)(earth_yaw+8))>>4]) ;	// heading
#endif
			
#if (OSD_LOC_VERTICAL_ANGLE_HOME != OSD_LOC_DISABLED)
			// Vertical angle from origin to plane
			int verticalAngle = 0 ;
			if (dist_to_goal != 0)
			{
				struct relative2D componentsToPlane ;
				componentsToPlane.x = dist_to_goal ;
				componentsToPlane.y = IMUlocationz._.W1 ;
				verticalAngle = rect_to_polar(&componentsToPlane) ;		// binary angle (0 - 256 = 360 degrees)
				verticalAngle = (verticalAngle * BYTECIR_TO_DEGREE) >> 16 ;	// switch polarity, convert to -180 - 180 degrees
			}
			osd_spi_write_location(OSD_LOC_VERTICAL_ANGLE_HOME) ;
			osd_spi_write_number(verticalAngle, 0, 0, NUM_FLAG_SIGNED, 0, 0x4D); // Footer: Degree symbol
#endif
			
			
#if (OSD_LOC_ROLL_RATE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_ROLL_RATE) ;
			osd_spi_write_number(abs(omegagyro[1])/DEGPERSEC, 3, 0, 0, 0, 0) ;	// roll rate in degrees/sec/sec
#endif
			
#if (OSD_LOC_PITCH_RATE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_PITCH_RATE) ;
			osd_spi_write_number(abs(omegagyro[0])/DEGPERSEC, 3, 0, 0, 0, 0) ;	// pitch rate in degrees/sec/sec
#endif
			
#if (OSD_LOC_YAW_RATE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_YAW_RATE) ;
			osd_spi_write_number(abs(omegagyro[2])/DEGPERSEC, 3, 0, 0, 0, 0) ;	// yaw rate in degrees/sec/sec
#endif
			
			
#if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED)
			
#if (OSD_LOC_BATT_CURRENT != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_BATT_CURRENT) ;
			osd_spi_write_number(battery_current._.W1, 3, 1, 0, 0, 0xB4) ;	// tenths of Amps being used right now
#endif
			
#if (OSD_LOC_BATT_USED != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_BATT_USED) ;
			osd_spi_write_number(battery_mAh_used._.W1, 4, 0, 0, 0, 0xB7) ;	// mAh used so far
#endif

#endif


#if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED)
			
#if (OSD_LOC_BATT_VOLTAGE != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_BATT_VOLTAGE) ;
			osd_spi_write_number(battery_voltage._.W1, 3, 1, 0, 0, 0xA0) ;	// tenths of Volts
#endif
			
#endif

			
#if (ANALOG_RSSI_INPUT_CHANNEL != CHANNEL_UNUSED)

#if (OSD_LOC_RSSI != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_RSSI) ;
			osd_spi_write_number(rc_signal_strength, 3, 0, 0, 0, 0xB3) ;	// RC Receiver signal strength as 0-100%
#endif

#endif
			break ;
		}
		case 2:
		{
#if (OSD_SHOW_HORIZON == 1)
			osd_update_horizon() ;
#endif
			break ;
		}
		case 3:
		{
#if (OSD_LOC_AIR_SPEED_M_S != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_AIR_SPEED_M_S) ;
			osd_spi_write_number(air_speed_3DIMU/100, 3, 0, 0, 0, 0) ;	// speed in m/s
#endif
			
#if (OSD_LOC_AIR_SPEED_MI_HR != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_AIR_SPEED_MI_HR) ;
			osd_spi_write_number(air_speed_3DIMU/45, 3, 0, 0, 0, 0) ;	// speed in mi/hr
#endif
			
#if (OSD_LOC_AIR_SPEED_KM_HR != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_AIR_SPEED_KM_HR) ;
			osd_spi_write_number(air_speed_3DIMU/28, 3, 0, 0, 0, 0) ;	// speed in km/hr
#endif
			
			
#if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED)
			unsigned int ground_speed_3DIMU = 
				vector3_mag ( 	IMUvelocityx._.W1 ,
								IMUvelocityy._.W1 ,
								IMUvelocityz._.W1   ) ;
#endif
			
#if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_GROUND_SPEED_M_S) ;
			osd_spi_write_number(ground_speed_3DIMU/100, 3, 0, 0, 0, 0) ;	// speed in m/s
#endif
			
#if (OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_GROUND_SPEED_MI_HR) ;
			osd_spi_write_number(ground_speed_3DIMU/45, 3, 0, 0, 0, 0) ;	// speed in mi/hr
#endif
			
#if (OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_GROUND_SPEED_KM_HR) ;
			osd_spi_write_number(ground_speed_3DIMU/28, 3, 0, 0, 0, 0) ;	// speed in km/hr
#endif
			
			
#if (OSD_LOC_VERTICAL_ACCEL != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_VERTICAL_ACCEL) ;
			union longww gravity_z ;
			gravity_z.WW = __builtin_mulss(GRAVITY, rmat[8]) << 2;
			osd_spi_write_number((ZACCEL_VALUE - gravity_z._.W1)/(100*ACCELSCALE), 3, 0, NUM_FLAG_SIGNED, 0, 0) ;	// vertical acceleration rate in units of m/sec/sec
#endif
			
			
#if (OSD_LOC_VERTICAL_WIND_SPEED != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_VERTICAL_WIND_SPEED) ;
			osd_spi_write_number(estimatedWind[2]/10, 4, 1, NUM_FLAG_SIGNED, 0, 0) ;	// vertical wind speed in m/s
#endif
			
			
#if (OSD_LOC_TOTAL_ENERGY != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_TOTAL_ENERGY) ;
			osd_spi_write_number(total_energy, 4, 0, NUM_FLAG_SIGNED, 0, 0) ;	// total energy in meters above the origin
#endif
			
			
#if (OSD_AUTO_HIDE_GPS == 1)
			boolean showGPS = ( IMUlocationz._.W1 < 20 || ground_velocity_magnitudeXY < 150) ;
#else
			boolean showGPS = 1 ;
#endif
			
#if (OSD_LOC_NUM_SATS != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_NUM_SATS) ;
			if (showGPS)
			{
				osd_spi_write_number(svs, 0, 0, 0, 0xEB, 0) ;			// Num satelites locked, with SatDish icon header
			}
			else
			{
				osd_spi_erase_chars(3) ;
			}
#endif
			
#if (OSD_LOC_GPS_LAT != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_GPS_LAT) ;
			if (showGPS)
			{
				osd_spi_write_number(labs(lat_gps.WW/10), 8, 6, 0, 0, (lat_gps.WW >= 0) ? 0x98 : 0x9D) ; // Footer: N/S
			}
			else
			{
				osd_spi_erase_chars(9) ;
			}
#endif
			
#if (OSD_LOC_GPS_LONG != OSD_LOC_DISABLED)
			osd_spi_write_location(OSD_LOC_GPS_LONG) ;
			if (showGPS)
			{
				osd_spi_write_number(labs(long_gps.WW/10), 9, 6, 0, 0, (long_gps.WW >= 0) ? 0x8F : 0xA1) ; // Footer: E/W
			}
			else
			{
				osd_spi_erase_chars(10) ;
			}
#endif
			break ;
		}
	}
	return ;
}
Esempio n. 3
0
void cmd_cpuload(void)
{
	printf("CPU Load %u%%\r\n", udb_cpu_load());
}
Esempio n. 4
0
void serial_output_8hz( void )
{
#if ( SERIAL_OUTPUT_FORMAT == SERIAL_UDB )	// Only run through this function twice per second, by skipping all but every 4 runs through it.
	// Saves CPU and XBee power.
	if (udb_heartbeat_counter % 20 != 0) return ;  // Every 4 runs (5 heartbeat counts per 8Hz)
	
#elif ( SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA )
	// SERIAL_UDB_EXTRA expected to be used with the OpenLog which can take greater transfer speeds than Xbee
	// F2: SERIAL_UDB_EXTRA format is printed out every other time, although it is being called at 8Hz, this
	//		version will output four F2 lines every second (4Hz updates)
#endif

	switch (telemetry_counter)
	{
		// The first lines of telemetry contain info about the compile-time settings from the options.h file
		case 6:
			if ( _SWR == 0 )
			{
				// if there was not a software reset (trap error) clear the trap data
				trap_flags = trap_source = osc_fail_count = 0 ;
			}
			serial_output("\r\nF14:WIND_EST=%i:GPS_TYPE=%i:DR=%i:BOARD_TYPE=%i:AIRFRAME=%i:RCON=0x%X:TRAP_FLAGS=0x%X:TRAP_SOURCE=0x%lX:ALARMS=%i:"  \
							"CLOCK=%i:\r\n",
				WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE, RCON , trap_flags , trap_source , osc_fail_count, CLOCK_CONFIG ) ;
				RCON = 0 ;
				trap_flags = 0 ;
				trap_source = 0 ;
				osc_fail_count = 0 ;
			break ;
		case 5:
			serial_output("F4:R_STAB_A=%i:R_STAB_RD=%i:P_STAB=%i:Y_STAB_R=%i:Y_STAB_A=%i:AIL_NAV=%i:RUD_NAV=%i:AH_STAB=%i:AH_WP=%i:RACE=%i:\r\n",
				ROLL_STABILIZATION_AILERONS, ROLL_STABILIZATION_RUDDER, PITCH_STABILIZATION, YAW_STABILIZATION_RUDDER, YAW_STABILIZATION_AILERON,
				AILERON_NAVIGATION, RUDDER_NAVIGATION, ALTITUDEHOLD_STABILIZED, ALTITUDEHOLD_WAYPOINT, RACING_MODE) ;
			break ;
		case 4:
			serial_output("F5:YAWKP_A=%5.3f:YAWKD_A=%5.3f:ROLLKP=%5.3f:ROLLKD=%5.3f:A_BOOST=%3.1f:\r\n",
				YAWKP_AILERON, YAWKD_AILERON, ROLLKP, ROLLKD, AILERON_BOOST ) ;
			break ;
		case 3:
			serial_output("F6:P_GAIN=%5.3f:P_KD=%5.3f:RUD_E_MIX=%5.3f:ROL_E_MIX=%5.3f:E_BOOST=%3.1f:\r\n",
				PITCHGAIN, PITCHKD, RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST) ;
			break ;
		case 2:
			serial_output("F7:Y_KP_R=%5.4f:Y_KD_R=%5.3f:RLKP_RUD=%5.3f:RUD_BOOST=%5.3f:RTL_PITCH_DN=%5.3f:\r\n",
				YAWKP_RUDDER, YAWKD_RUDDER, ROLLKP_RUDDER , RUDDER_BOOST, RTL_PITCH_DOWN) ;
			break ;
		case 1:
			serial_output("F8:H_MAX=%6.1f:H_MIN=%6.1f:MIN_THR=%3.2f:MAX_THR=%3.2f:PITCH_MIN_THR=%4.1f:PITCH_MAX_THR=%4.1f:PITCH_ZERO_THR=%4.1f:\r\n",
				HEIGHT_TARGET_MAX, HEIGHT_TARGET_MIN, ALT_HOLD_THROTTLE_MIN, ALT_HOLD_THROTTLE_MAX,
				ALT_HOLD_PITCH_MIN, ALT_HOLD_PITCH_MAX, ALT_HOLD_PITCH_HIGH) ;
			break ;
		default:
		{
			// F2 below means "Format Revision 2: and is used by a Telemetry parser to invoke the right pattern matching
			// F2 is a compromise between easy reading of raw data in a file and not droppping chars in transmission.
			
#if ( SERIAL_OUTPUT_FORMAT == SERIAL_UDB )
			serial_output("F2:T%li:S%d%d%d:N%li:E%li:A%li:W%i:a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i:i%i:c%u:s%i:cpu%u:bmv%i:"
				"as%i:wvx%i:wvy%i:wvz%i:\r\n",
				tow.WW, udb_flags._.radio_on, dcm_flags._.nav_capable, flags._.GPS_steering,
				lat_gps.WW , long_gps.WW , alt_sl_gps.WW, waypointIndex,
				rmat[0] , rmat[1] , rmat[2] ,
				rmat[3] , rmat[4] , rmat[5] ,
				rmat[6] , rmat[7] , rmat[8] ,
				(unsigned int)cog_gps.BB, sog_gps.BB, (unsigned int)udb_cpu_load(), voltage_milis.BB,
				air_speed_3DIMU, 
				estimatedWind[0], estimatedWind[1], estimatedWind[2] ) ;
			// Approximate time passing between each telemetry line, even though
			// we may not have new GPS time data each time through.
			if (tow.WW > 0) tow.WW += 500 ;
				
#elif ( SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA )
			if (udb_heartbeat_counter % 10 != 0)  // Every 2 runs (5 heartbeat counts per 8Hz)
			{
					serial_output("F2:T%li:S%d%d%d:N%li:E%li:A%li:W%i:a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i:i%i:c%u:s%i:cpu%u:bmv%i:"
					"as%u:wvx%i:wvy%i:wvz%i:ma%i:mb%i:mc%i:svs%i:hd%i:",
					tow.WW, udb_flags._.radio_on, dcm_flags._.nav_capable, flags._.GPS_steering,
					lat_gps.WW , long_gps.WW , alt_sl_gps.WW, waypointIndex,
					rmat[0] , rmat[1] , rmat[2] ,
					rmat[3] , rmat[4] , rmat[5] ,
					rmat[6] , rmat[7] , rmat[8] ,
					(unsigned int)cog_gps.BB, sog_gps.BB, (unsigned int)udb_cpu_load(), voltage_milis.BB,
					air_speed_3DIMU,
					estimatedWind[0], estimatedWind[1], estimatedWind[2],
#if (MAG_YAW_DRIFT == 1)
					magFieldEarth[0],magFieldEarth[1],magFieldEarth[2],
#else
					(int)0, (int)0, (int)0,
#endif
					
					svs, hdop ) ;
				
				// Approximate time passing between each telemetry line, even though
				// we may not have new GPS time data each time through.
				if (tow.WW > 0) tow.WW += 250 ; 
				
				// Save  pwIn and PwOut buffers for printing next time around
				int i ;
				for (i=0; i <= NUM_INPUTS; i++)
					pwIn_save[i] = udb_pwIn[i] ;
				for (i=0; i <= NUM_OUTPUTS; i++)
					pwOut_save[i] = udb_pwOut[i] ;
			}
			else
			{
				int i ;
				for (i= 1; i <= NUM_INPUTS; i++)
					serial_output("p%ii%i:",i,pwIn_save[i]);
				for (i= 1; i <= NUM_OUTPUTS; i++)
					serial_output("p%io%i:",i,pwOut_save[i]);
				serial_output("imx%i:imy%i:imz%i:fgs%X:ofc%i:",IMUlocationx._.W1 ,IMUlocationy._.W1 ,IMUlocationz._.W1,
					 flags.WW, osc_fail_count );
#if (RECORD_FREE_STACK_SPACE == 1)
				serial_output("stk%d:", (int)(4096-maxstack));
#endif
				serial_output("\r\n");
			}
#endif
			if (flags._.f13_print_req == 1)
			{
				// The F13 line of telemetry is printed when origin has been captured and inbetween F2 lines in SERIAL_UDB_EXTRA
#if ( SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA )
				if (udb_heartbeat_counter % 10 != 0) return ;
#endif
				serial_output("F13:week%i:origN%li:origE%li:origA%li:\r\n", week_no, lat_origin.WW, long_origin.WW, alt_origin) ;
				flags._.f13_print_req = 0 ;
			}
			
			return ;
		}
	}
	telemetry_counter-- ;
	return ;
}
Esempio n. 5
0
void serial_output_8hz(void)
{
#if (SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA)
	int16_t i;
	static int toggle = 0;
#endif
//	static int16_t telemetry_counter = 8;
#if (SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA)
	// SERIAL_UDB_EXTRA expected to be used with the OpenLog which can take greater transfer speeds than Xbee
	// F2: SERIAL_UDB_EXTRA format is printed out every other time, although it is being called at 8Hz, this
	//     version will output four F2 lines every second (4Hz updates)
	static int16_t pwIn_save[NUM_INPUTS + 1];
	static int16_t pwOut_save[NUM_OUTPUTS + 1];
#elif (SERIAL_OUTPUT_FORMAT == SERIAL_UDB)          // Only run through this function twice per second, by skipping all but every 4 runs through it.
	// Saves CPU and XBee power.
	if (udb_heartbeat_counter % 20 != 0) return;    // Every 4 runs (5 heartbeat counts per 8Hz)
#endif // SERIAL_OUTPUT_FORMAT

	switch (telemetry_counter)
	{
		// The first lines of telemetry contain info about the compile-time settings from the options.h file
		case 8:
			serial_output("\r\nF14:WIND_EST=%i:GPS_TYPE=%i:DR=%i:BOARD_TYPE=%i:AIRFRAME=%i:"
			              "RCON=0x%X:TRAP_FLAGS=0x%X:TRAP_SOURCE=0x%lX:ALARMS=%i:"  \
			              "CLOCK=%i:FP=%d:\r\n",
			    WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE,
			    get_reset_flags(), trap_flags, trap_source, osc_fail_count,
			    CLOCK_CONFIG, FLIGHT_PLAN_TYPE);
			break;
		case 7:
			serial_output("F15:IDA=");
			serial_output(ID_VEHICLE_MODEL_NAME);
			serial_output(":IDB=");
			serial_output(ID_VEHICLE_REGISTRATION);
			serial_output(":\r\n");
			break;
		case 6:
			serial_output("F16:IDC=");
			serial_output(ID_LEAD_PILOT);
			serial_output(":IDD=");
			serial_output(ID_DIY_DRONES_URL);
			serial_output(":\r\n");
			break;
		case 5:
			serial_output("F4:R_STAB_A=%i:R_STAB_RD=%i:P_STAB=%i:Y_STAB_R=%i:Y_STAB_A=%i:AIL_NAV=%i:RUD_NAV=%i:AH_STAB=%i:AH_WP=%i:RACE=%i:\r\n",
			    ROLL_STABILIZATION_AILERONS, ROLL_STABILIZATION_RUDDER, PITCH_STABILIZATION, YAW_STABILIZATION_RUDDER, YAW_STABILIZATION_AILERON,
			    AILERON_NAVIGATION, RUDDER_NAVIGATION, ALTITUDEHOLD_STABILIZED, ALTITUDEHOLD_WAYPOINT, RACING_MODE);
			break;
		case 4:
			serial_output("F5:YAWKP_A=%5.3f:YAWKD_A=%5.3f:ROLLKP=%5.3f:ROLLKD=%5.3f:A_BOOST=%3.1f:\r\n",
			    YAWKP_AILERON, YAWKD_AILERON, ROLLKP, ROLLKD, AILERON_BOOST);
			break;
		case 3:
			serial_output("F6:P_GAIN=%5.3f:P_KD=%5.3f:RUD_E_MIX=%5.3f:ROL_E_MIX=%5.3f:E_BOOST=%3.1f:\r\n",
			    PITCHGAIN, PITCHKD, RUDDER_ELEV_MIX, ROLL_ELEV_MIX, ELEVATOR_BOOST);
			break;
		case 2:
			serial_output("F7:Y_KP_R=%5.4f:Y_KD_R=%5.3f:RLKP_RUD=%5.3f:RLKD_RUD=%5.3f:RUD_BOOST=%5.3f:RTL_PITCH_DN=%5.3f:\r\n",
			    YAWKP_RUDDER, YAWKD_RUDDER, ROLLKP_RUDDER, ROLLKD_RUDDER, RUDDER_BOOST, RTL_PITCH_DOWN);
			break;
		case 1:
			serial_output("F8:H_MAX=%6.1f:H_MIN=%6.1f:MIN_THR=%3.2f:MAX_THR=%3.2f:PITCH_MIN_THR=%4.1f:PITCH_MAX_THR=%4.1f:PITCH_ZERO_THR=%4.1f:\r\n",
			    HEIGHT_TARGET_MAX, HEIGHT_TARGET_MIN, ALT_HOLD_THROTTLE_MIN, ALT_HOLD_THROTTLE_MAX,
			    ALT_HOLD_PITCH_MIN, ALT_HOLD_PITCH_MAX, ALT_HOLD_PITCH_HIGH);
			break;
		default:
		{
			// F2 below means "Format Revision 2: and is used by a Telemetry parser to invoke the right pattern matching
			// F2 is a compromise between easy reading of raw data in a file and not droppping chars in transmission.
#if (SERIAL_OUTPUT_FORMAT == SERIAL_UDB)
			serial_output("F2:T%li:S%d%d%d:N%li:E%li:A%li:W%i:a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i:i%i:c%u:s%i:cpu%u:bmv%i:"
			              "as%i:wvx%i:wvy%i:wvz%i:\r\n",
			    tow.WW, udb_flags._.radio_on, dcm_flags._.nav_capable, flags._.GPS_steering,
			    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
			    rmat[0], rmat[1], rmat[2],
			    rmat[3], rmat[4], rmat[5],
			    rmat[6], rmat[7], rmat[8],
			    (uint16_t)cog_gps.BB, sog_gps.BB, (uint16_t)udb_cpu_load(), voltage_milis.BB,
			    air_speed_3DIMU, 
			    estimatedWind[0], estimatedWind[1], estimatedWind[2]);
			// Approximate time passing between each telemetry line, even though
			// we may not have new GPS time data each time through.
			if (tow.WW > 0) tow.WW += 500;

#elif (SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA)
//			if (udb_heartbeat_counter % 10 != 0)                // Every 2 runs (5 heartbeat counts per 8Hz)
//			if (udb_heartbeat_counter % (HEARTBEAT_HZ/4) != 0)  // Every 2 runs (5 heartbeat counts per 8Hz)

			toggle = !toggle;

			if (toggle)
			{
				serial_output("F2:T%li:S%d%d%d:N%li:E%li:A%li:W%i:"
				              "a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i:i%i:"
				              "c%u:s%i:cpu%u:bmv%i:"
				              "as%u:wvx%i:wvy%i:wvz%i:ma%i:mb%i:mc%i:svs%i:hd%i:",
				    tow.WW, udb_flags._.radio_on, dcm_flags._.nav_capable, flags._.GPS_steering,
				    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
				    rmat[0], rmat[1], rmat[2],
				    rmat[3], rmat[4], rmat[5],
				    rmat[6], rmat[7], rmat[8],
				    (uint16_t)cog_gps.BB, sog_gps.BB, (uint16_t)udb_cpu_load(), voltage_milis.BB,
				    air_speed_3DIMU,
				    estimatedWind[0], estimatedWind[1], estimatedWind[2],
#if (MAG_YAW_DRIFT == 1)
				    magFieldEarth[0], magFieldEarth[1], magFieldEarth[2],
#else
				    (int16_t)0, (int16_t)0, (int16_t)0,
#endif // MAG_YAW_DRIFT
				    svs, hdop);

				// Approximate time passing between each telemetry line, even though
				// we may not have new GPS time data each time through.
				if (tow.WW > 0) tow.WW += 250; 

				// Save  pwIn and PwOut buffers for printing next time around
				for (i = 0; i <= NUM_INPUTS; i++)
					pwIn_save[i] = udb_pwIn[i];
				for (i = 0; i <= NUM_OUTPUTS; i++)
					pwOut_save[i] = udb_pwOut[i];
			}
			else
			{
				int16_t i;
				for (i= 1; i <= NUM_INPUTS; i++)
					serial_output("p%ii%i:",i,pwIn_save[i]);
				for (i= 1; i <= NUM_OUTPUTS; i++)
					serial_output("p%io%i:",i,pwOut_save[i]);
				serial_output("imx%i:imy%i:imz%i:lex%i:ley%i:lez%i:fgs%X:ofc%i:tx%i:ty%i:tz%i:G%d,%d,%d:",IMUlocationx._.W1,IMUlocationy._.W1,IMUlocationz._.W1,
				    locationErrorEarth[0], locationErrorEarth[1], locationErrorEarth[2],
				    flags.WW, osc_fail_count,
				    IMUvelocityx._.W1, IMUvelocityy._.W1, IMUvelocityz._.W1, goal.x, goal.y, goal.height);
//				serial_output("tmp%i:prs%li:alt%li:agl%li:",
//				    get_barometer_temperature(), get_barometer_pressure(), 
//				    get_barometer_alt(), get_barometer_agl());
#if (RECORD_FREE_STACK_SPACE == 1)
				extern uint16_t maxstack;
				serial_output("stk%d:", (int16_t)(4096-maxstack));
#endif // RECORD_FREE_STACK_SPACE
				serial_output("\r\n");
			}

#elif (SERIAL_OUTPUT_FORMAT == SERIAL_UDB_MAG)
extern int16_t magFieldRaw[3];
extern int16_t udb_magOffset[3];

			serial_output("OFFx %i : OFFy %i : OFFz %i : RAWx %i : RAWy %i : RAWz %i :",
			    udb_magOffset[0], udb_magOffset[1], udb_magOffset[2],
			    magFieldRaw[0], magFieldRaw[1], magFieldRaw[2]);
			    serial_output("\r\n");

#endif // SERIAL_OUTPUT_FORMAT
			if (flags._.f13_print_req == 1)
			{
				// The F13 line of telemetry is printed when origin has been captured and inbetween F2 lines in SERIAL_UDB_EXTRA
#if (SERIAL_OUTPUT_FORMAT == SERIAL_UDB_EXTRA)
				if (udb_heartbeat_counter % 10 != 0) return;
#endif
				serial_output("F13:week%i:origN%li:origE%li:origA%li:\r\n", week_no, lat_origin.WW, lon_origin.WW, alt_origin);
				flags._.f13_print_req = 0;
			}
			break;
		}
	}
	if (telemetry_counter)
	{
		telemetry_counter--;
	}
#if (USE_TELELOG == 1)
	log_swapbuf();
#endif
}
Esempio n. 6
0
void MAVUDBExtraOutput_40hz(void)
{
	// SEND SERIAL_UDB_EXTRA (SUE) VIA MAVLINK FOR BACKWARDS COMPATIBILITY with FLAN.PYW (FLIGHT ANALYZER)
	// The MAVLink messages for this section of code are unique to MatrixPilot and are defined in matrixpilot.xml
//	spread_transmission_load = 10;
//	if (mavlink_frequency_send(streamRates[MAV_DATA_STREAM_EXTRA1], mavlink_counter_40hz + spread_transmission_load)) // SUE code historically ran at 8HZ
	{
		switch (mavlink_sue_telemetry_counter)
		{
			case 8:
				mavlink_msg_serial_udb_extra_f14_send(MAVLINK_COMM_0, WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE,
				    get_reset_flags(), trap_flags, trap_source, osc_fail_count, CLOCK_CONFIG, FLIGHT_PLAN_TYPE);
				mavlink_sue_telemetry_counter--;
				break;
			case 7:
				mavlink_msg_serial_udb_extra_f15_send(MAVLINK_COMM_0, (uint8_t*)ID_VEHICLE_MODEL_NAME, (uint8_t*)ID_VEHICLE_REGISTRATION);
				mavlink_sue_telemetry_counter--;
				break;
			case 6:
				mavlink_msg_serial_udb_extra_f16_send(MAVLINK_COMM_0, (uint8_t*)ID_LEAD_PILOT, (uint8_t*)ID_DIY_DRONES_URL);
				mavlink_sue_telemetry_counter--;
				break;
			case 5:
				mavlink_msg_serial_udb_extra_f4_send(MAVLINK_COMM_0, settings._.RollStabilizaionAilerons, settings._.RollStabilizationRudder, settings._.PitchStabilization,
				    settings._.YawStabilizationRudder, settings._.YawStabilizationAileron, settings._.AileronNavigation, settings._.RudderNavigation, settings._.AltitudeholdStabilized,
				    settings._.AltitudeholdWaypoint, settings._.RacingMode);
				mavlink_sue_telemetry_counter--;
				break;
			case 4:
				mavlink_msg_serial_udb_extra_f5_send(MAVLINK_COMM_0, gains.YawKPAileron, gains.YawKDAileron, gains.RollKP, gains.RollKD,
				    settings._.YawStabilizationAileron, gains.AileronBoost);
				mavlink_sue_telemetry_counter--;
				break;
			case 3:
				mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_0, gains.Pitchgain, gains.PitchKD, gains.RudderElevMix, gains.RollElevMix, gains.ElevatorBoost);
				mavlink_sue_telemetry_counter--;
				break;
			case 2:
				mavlink_msg_serial_udb_extra_f7_send(MAVLINK_COMM_0, gains.YawKPRudder, gains.YawKDRudder, gains.RollKPRudder, gains.RollKDRudder,
				    gains.RudderBoost, gains.RtlPitchDown);
				mavlink_sue_telemetry_counter--;
				break;
			case 1:
				mavlink_msg_serial_udb_extra_f8_send(MAVLINK_COMM_0, altit.HeightTargetMax, altit.HeightTargetMin, altit.AltHoldThrottleMin,
				    altit.AltHoldThrottleMax, altit.AltHoldPitchMin, altit.AltHoldPitchMax, altit.AltHoldPitchHigh);
				mavlink_sue_telemetry_counter--;
				break;
			default:
			{
				if (mavlink_sue_telemetry_f2_a == true)
				{
					int16_t i;

					mavlink_sue_telemetry_f2_a = false;
					// Approximate time passing between each telemetry line, even though
					// we may not have new GPS time data each time through.
					// This line is important when GPS lock is lost during flight
					// It allows telemetry to have a time reference when the GPS time reference is lost
					// Note this does increment the official Time of Week (TOW) for the entire system.
					// It is not changed for now, to preserve close compatibility with origin SERIAL_UDB_EXTRA code.
					if (tow.WW > 0) tow.WW += 250;

					if (state_flags._.f13_print_req == 1)
					{
						// The F13 line of telemetry is printed just once  when origin has been captured after GPS lock
						mavlink_msg_serial_udb_extra_f13_send(MAVLINK_COMM_0, week_no.BB, lat_origin.WW, lon_origin.WW, alt_origin.WW);
						state_flags._.f13_print_req = 0;
					}
#if (MAG_YAW_DRIFT == 1)
					mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_0, tow.WW,
					    ((udb_flags._.radio_on << 2) + (dcm_flags._.nav_capable << 1) + state_flags._.GPS_steering),
					    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
					    rmat[0], rmat[1], rmat[2], rmat[3], rmat[4], rmat[5], rmat[6], rmat[7], rmat[8],
					    (uint16_t) cog_gps.BB, sog_gps.BB, (uint16_t) udb_cpu_load(), voltage_milis.BB,
					    air_speed_3DIMU, estimatedWind[0], estimatedWind[1], estimatedWind[2],
					    magFieldEarth[0], magFieldEarth[1], magFieldEarth[2],
					    svs, hdop);
#else
					mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_0, tow.WW,
					    ((udb_flags._.radio_on << 2) + (dcm_flags._.nav_capable << 1) + state_flags._.GPS_steering),
					    lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
					    rmat[0], rmat[1], rmat[2], rmat[3], rmat[4], rmat[5], rmat[6], rmat[7], rmat[8],
					    (uint16_t) cog_gps.BB, sog_gps.BB, (uint16_t) udb_cpu_load(), voltage_milis.BB,
					    air_speed_3DIMU, estimatedWind[0], estimatedWind[1], estimatedWind[2],
					    0, 0, 0,
					    svs, hdop);
#endif // (MAG_YAW_DRIFT == 1)
					// Save  pwIn and PwOut buffers for sending next time around in f2_b format message
					for (i = 0; i <= (NUM_INPUTS > MAVLINK_SUE_CHANNEL_MAX_SIZE ? MAVLINK_SUE_CHANNEL_MAX_SIZE : NUM_INPUTS); i++)
						pwIn_save[i] = udb_pwIn[i];
					for (i = 0; i <= (NUM_OUTPUTS > MAVLINK_SUE_CHANNEL_MAX_SIZE ? MAVLINK_SUE_CHANNEL_MAX_SIZE : NUM_OUTPUTS); i++)
						pwOut_save[i] = udb_pwOut[i];
					}
					else
					{
						vect3_16t goal;
						int16_t stack_free = 0;
						mavlink_sue_telemetry_f2_a = true;
#if (RECORD_FREE_STACK_SPACE == 1)
						stack_free = (int16_t)(4096-maxstack); // This is actually wrong for the UDB4, but currently left the same as for telemetry.c
#endif // (RECORD_FREE_STACK_SPACE == 1)

//void navigate_get_goal(vect3_16t* goal);
						navigate_get_goal(&goal);

						mavlink_msg_serial_udb_extra_f2_b_send(MAVLINK_COMM_0, tow.WW,
						    pwIn_save[1], pwIn_save[2], pwIn_save[3], pwIn_save[4], pwIn_save[5],
						    pwIn_save[6], pwIn_save[7], pwIn_save[8], pwIn_save[9], pwIn_save[10],
						    pwOut_save[1], pwOut_save[2], pwOut_save[3], pwOut_save[4], pwOut_save[5],
						    pwOut_save[6], pwOut_save[7], pwOut_save[8], pwOut_save[9], pwOut_save[10],
						    IMUlocationx._.W1, IMUlocationy._.W1, IMUlocationz._.W1, state_flags.WW,
#if (SILSIM != 1)
						    osc_fail_count,
#else
						    0,
#endif // (SILSIM != 1)
						    IMUvelocityx._.W1, IMUvelocityy._.W1, IMUvelocityz._.W1,
						    goal.x, goal.y, goal.z, stack_free);
					}
				}
		}
	}
}
Esempio n. 7
0
void MAVUDBExtraOutput(void)
{
	// SEND SERIAL_UDB_EXTRA (SUE) VIA MAVLINK FOR BACKWARDS COMPATIBILITY with FLAN.PYW (FLIGHT ANALYZER)
	// SUE messages have important MatrixPilot specific information like cause of reboots e.g. power brownout.
	// The MAVLink messages for this section of code are defined in
	// Tools/MAVLink/mavlink/pymavlink/message_definitions/V1.0/matrixpilot.xml
	int16_t i;
	static int mavlink_sue_toggle = 0;
	static boolean f13_print_prepare = false;
	
	// Following are required for saving state of PWM variables for SERIAL_UDB_EXTRA compatibility
	static int16_t pwIn_save[MAVLINK_SUE_CHANNEL_MAX_SIZE + 1];
	static int16_t pwOut_save[MAVLINK_SUE_CHANNEL_MAX_SIZE + 1];
	static int16_t pwTrim_save[MAVLINK_SUE_CHANNEL_MAX_SIZE + 1];
	
	switch  (mavlink_sue_telemetry_counter)
	{
		case 13:
//			serial_output("F22:Sensors=%i,%i,%i,%i,%i,%i\n",
//				UDB_XACCEL.value, UDB_YACCEL.value,
//				UDB_ZACCEL.value + (Z_GRAVITY_SIGN ((int16_t)(2*GRAVITY))),
//				udb_xrate.value, udb_yrate.value, udb_zrate.value);
			mavlink_msg_serial_udb_extra_f22_send(MAVLINK_COMM_0,
				UDB_XACCEL.value, UDB_YACCEL.value,
				UDB_ZACCEL.value + (Z_GRAVITY_SIGN ((int16_t)(2*GRAVITY))),
				udb_xrate.value, udb_yrate.value, udb_zrate.value);
			break;
		case 12: 
//			serial_output("F21:Offsets=%i,%i,%i,%i,%i,%i\n",
//				UDB_XACCEL.offset, UDB_YACCEL.offset, UDB_ZACCEL.offset,
//				udb_xrate.offset, udb_yrate.offset, udb_zrate.offset);
			mavlink_msg_serial_udb_extra_f21_send(MAVLINK_COMM_0,
				UDB_XACCEL.offset, UDB_YACCEL.offset, UDB_ZACCEL.offset,
				udb_xrate.offset, udb_yrate.offset, udb_zrate.offset);
			break;
		case 11:
//			serial_output("F15:IDA=");
//			serial_output(ID_VEHICLE_MODEL_NAME);
//			serial_output(":IDB=");
//			serial_output(ID_VEHICLE_REGISTRATION);
//			serial_output(":\r\n");
			mavlink_msg_serial_udb_extra_f15_send(MAVLINK_COMM_0,
				(uint8_t*)ID_VEHICLE_MODEL_NAME, 
				(uint8_t*)ID_VEHICLE_REGISTRATION);
			break;
		case 10:
//			serial_output("F16:IDC=");
//			serial_output(ID_LEAD_PILOT);
//			serial_output(":IDD=");
//			serial_output(ID_DIY_DRONES_URL);
//			serial_output(":\r\n");
			mavlink_msg_serial_udb_extra_f16_send(MAVLINK_COMM_0, 
				(uint8_t*)ID_LEAD_PILOT,
				(uint8_t*)ID_DIY_DRONES_URL);
			break;
		case 9:
//			serial_output("F17:FD_FWD=%5.3f:TR_NAV=%5.3f:TR_FBW=%5.3f:\r\n",
//				turns.FeedForward, turns.TurnRateNav, turns.TurnRateFBW);
			mavlink_msg_serial_udb_extra_f17_send(MAVLINK_COMM_0,
				turns.FeedForward, turns.TurnRateNav, turns.TurnRateFBW);
			break;
		case 8:
//			serial_output("F18:AOA_NRM=%5.3f:AOA_INV=%5.3f:EL_TRIM_NRM=%5.3f:EL_TRIM_INV=%5.3f:CRUISE_SPD=%5.3f:\r\n",
//				turns.AngleOfAttackNormal, turns.AngleOfAttackInverted, turns.ElevatorTrimNormal,
//				turns.ElevatorTrimInverted, turns.RefSpeed);
			mavlink_msg_serial_udb_extra_f18_send(MAVLINK_COMM_0,
				turns.AngleOfAttackNormal, turns.AngleOfAttackInverted, turns.ElevatorTrimNormal,
				turns.ElevatorTrimInverted, turns.RefSpeed);
			break;
		case 7:
//			serial_output("F19:AIL=%i,%i:ELEV=%i,%i:THROT=%i,%i:RUDD=%i,%i:\r\n",
//				AILERON_OUTPUT_CHANNEL, AILERON_CHANNEL_REVERSED, ELEVATOR_OUTPUT_CHANNEL,ELEVATOR_CHANNEL_REVERSED,
//				THROTTLE_OUTPUT_CHANNEL, THROTTLE_CHANNEL_REVERSED, RUDDER_OUTPUT_CHANNEL,RUDDER_CHANNEL_REVERSED );
			mavlink_msg_serial_udb_extra_f19_send(MAVLINK_COMM_0,
				AILERON_OUTPUT_CHANNEL, AILERON_CHANNEL_REVERSED, ELEVATOR_OUTPUT_CHANNEL,ELEVATOR_CHANNEL_REVERSED,
				THROTTLE_OUTPUT_CHANNEL, THROTTLE_CHANNEL_REVERSED, RUDDER_OUTPUT_CHANNEL,RUDDER_CHANNEL_REVERSED);
			break;
		case 6:
//			serial_output("F14:WIND_EST=%i:GPS_TYPE=%i:DR=%i:BOARD_TYPE=%i:AIRFRAME=%i:"
//			              "RCON=0x%X:TRAP_FLAGS=0x%X:TRAP_SOURCE=0x%lX:ALARMS=%i:"
//			              "CLOCK=%i:FP=%d:\r\n",
//				WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE,
//				get_reset_flags(), trap_flags, trap_source, osc_fail_count,
//				CLOCK_CONFIG, FLIGHT_PLAN_TYPE);
			mavlink_msg_serial_udb_extra_f14_send(MAVLINK_COMM_0, 
				WIND_ESTIMATION, GPS_TYPE, DEADRECKONING, BOARD_TYPE, AIRFRAME_TYPE,
				get_reset_flags(), trap_flags, trap_source, osc_fail_count,
				CLOCK_CONFIG, FLIGHT_PLAN_TYPE);
			break;
		case 5:
//			serial_output("F4:R_STAB_A=%i:R_STAB_RD=%i:P_STAB=%i:Y_STAB_R=%i:Y_STAB_A=%i:AIL_NAV=%i:RUD_NAV=%i:AH_STAB=%i:AH_WP=%i:RACE=%i:\r\n",
//				settings._.RollStabilizaionAilerons, settings._.RollStabilizationRudder, 
//				settings._.PitchStabilization, settings._.YawStabilizationRudder, 
//				settings._.YawStabilizationAileron, settings._.AileronNavigation,
//				settings._.RudderNavigation, settings._.AltitudeholdStabilized,
//				settings._.AltitudeholdWaypoint, settings._.RacingMode);
			mavlink_msg_serial_udb_extra_f4_send(MAVLINK_COMM_0, 
				settings._.RollStabilizaionAilerons, settings._.RollStabilizationRudder,
				settings._.PitchStabilization, settings._.YawStabilizationRudder,
				settings._.YawStabilizationAileron, settings._.AileronNavigation,
				settings._.RudderNavigation, settings._.AltitudeholdStabilized,
				settings._.AltitudeholdWaypoint, settings._.RacingMode);
			break;
		case 4:
//			serial_output("F5:YAWKP_A=%5.3f:YAWKD_A=%5.3f:ROLLKP=%5.3f:ROLLKD=%5.3f:A_BOOST=%5.3f:A_BOOST=NULL\r\n",
//				gains.YawKPAileron, gains.YawKDAileron, gains.RollKP, gains.RollKD);
			mavlink_msg_serial_udb_extra_f5_send(MAVLINK_COMM_0, 
				gains.YawKPAileron, gains.YawKDAileron, gains.RollKP, gains.RollKD);
			break;
		case 3:
//			serial_output("F6:P_GAIN=%5.3f:P_KD=%5.3f:RUD_E_MIX=NULL:ROL_E_MIX=NULL:E_BOOST=%3.1f:\r\n",
//				gains.Pitchgain, gains.PitchKD, gains.ElevatorBoost);
			mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_0,
				gains.Pitchgain, gains.PitchKD, 0, 0, gains.ElevatorBoost);
			break;
		case 2:
//			serial_output("F7:Y_KP_R=%5.4f:Y_KD_R=%5.3f:RLKP_RUD=%5.3f:RLKD_RUD=%5.3f:RUD_BOOST=%5.3f:RTL_PITCH_DN=%5.3f:\r\n",
//				gains.YawKPRudder, gains.YawKDRudder, gains.RollKPRudder, gains.RollKDRudder, gains.RudderBoost, gains.RtlPitchDown);
			mavlink_msg_serial_udb_extra_f7_send(MAVLINK_COMM_0,
				gains.YawKPRudder, gains.YawKDRudder, gains.RollKPRudder, gains.RollKDRudder, gains.RudderBoost, gains.RtlPitchDown);
			break;
		case 1:
//			serial_output("F8:H_MAX=%6.1f:H_MIN=%6.1f:MIN_THR=%3.2f:MAX_THR=%3.2f:PITCH_MIN_THR=%4.1f:PITCH_MAX_THR=%4.1f:PITCH_ZERO_THR=%4.1f:\r\n",
//				altit.HeightTargetMax, altit.HeightTargetMin, altit.AltHoldThrottleMin, altit.AltHoldThrottleMax,
//				altit.AltHoldPitchMin, altit.AltHoldPitchMax, altit.AltHoldPitchHigh);
			mavlink_msg_serial_udb_extra_f8_send(MAVLINK_COMM_0,
				altit.HeightTargetMax, altit.HeightTargetMin, altit.AltHoldThrottleMin, altit.AltHoldThrottleMax,
				altit.AltHoldPitchMin, altit.AltHoldPitchMax, altit.AltHoldPitchHigh);
			break;
		default:
		{
			// F2 below means "Format Revision 2: and is used by a Telemetry parser to invoke the right pattern matching
			// F2 is a compromise between easy reading of raw data in an ascii file and minimising extraneous data in the stream.
			
			mavlink_sue_toggle = !mavlink_sue_toggle;
			if (state_flags._.f13_print_req == 1)
			{
				if (mavlink_sue_toggle && !f13_print_prepare)
				{
					f13_print_prepare = true;
					return;  //wait for next run
				}
 			}
			if (!f13_print_prepare)
			{
				if (mavlink_sue_toggle)
				{
//					serial_output("F2:T%li:S%d%d%d:N%li:E%li:A%li:W%i:"
//					              "a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i:i%i:"
//					              "c%u:s%i:cpu%u:"
//					              "as%u:wvx%i:wvy%i:wvz%i:ma%i:mb%i:mc%i:svs%i:hd%i:",
//						tow.WW, udb_flags._.radio_on, dcm_flags._.nav_capable, state_flags._.GPS_steering,
//						lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
//						rmat[0], rmat[1], rmat[2],
//						rmat[3], rmat[4], rmat[5],
//						rmat[6], rmat[7], rmat[8],
//						(uint16_t)cog_gps.BB, sog_gps.BB, (uint16_t)udb_cpu_load(), 
//						air_speed_3DIMU,
//						estimatedWind[0], estimatedWind[1], estimatedWind[2],
//#if (MAG_YAW_DRIFT == 1)
//						magFieldEarth[0], magFieldEarth[1], magFieldEarth[2],
//#else
//						(int16_t)0, (int16_t)0, (int16_t)0,
//#endif // MAG_YAW_DRIFT
//						svs, hdop);
					
					mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_0, 
						tow.WW, ((udb_flags._.radio_on << 2) + (dcm_flags._.nav_capable << 1) + state_flags._.GPS_steering),
						lat_gps.WW, lon_gps.WW, alt_sl_gps.WW, waypointIndex,
						rmat[0], rmat[1], rmat[2],
						rmat[3], rmat[4], rmat[5],
						rmat[6], rmat[7], rmat[8],
						(uint16_t) cog_gps.BB, sog_gps.BB, (uint16_t) udb_cpu_load(),
						air_speed_3DIMU, estimatedWind[0], estimatedWind[1], estimatedWind[2],
#if (MAG_YAW_DRIFT == 1)
						magFieldEarth[0], magFieldEarth[1], magFieldEarth[2],
#else
						(int16_t)0, (int16_t)0, (int16_t)0,
#endif
						svs, hdop);

					
					// Approximate time passing between each telemetry line, even though
					// we may not have new GPS time data each time through.
					// This line is important when GPS lock is lost during flight
					// It allows telemetry to have a time reference when the GPS time reference is lost
					// Note this does increment the official Time of Week (TOW) for the entire system,
					
					// The following code line assumes an update rate of 4HZ, (MAVUDBExtra() called at 8 HZ))
					// It is not changed for now, to preserve close compatibility with SERIAL_UDB_EXTRA code.

					if (tow.WW > 0) tow.WW += 250; 

					// Save  pwIn and PwOut buffers for printing next time around
					// Save  pwIn and PwOut buffers for sending next time around in f2_b format message
					for (i = 0; i <= MAVLINK_SUE_CHANNEL_MAX_SIZE; i++)
					{
						if (i <= NUM_INPUTS) 
						{
							pwIn_save[i] = udb_pwIn[i];
							pwTrim_save[i] = udb_pwTrim[i];
						}
						else
						{
							pwIn_save[i] = 0;
							pwTrim_save[i] = 0;
						}
						if (i <= NUM_OUTPUTS) 
						{
							pwOut_save[i] = udb_pwOut[i];
						}
						else
						{
							pwOut_save[i] = 0;
						}
					}	
				}
				else
				{
					vect3_16t goal;
					navigate_get_goal(&goal);
					int16_t stack_free = 0;
//					for (i= 1; i <= NUM_INPUTS; i++)
//						serial_output("p%ii%i:",i,pwIn_save[i]);
//					for (i= 1; i <= NUM_OUTPUTS; i++)
//						serial_output("p%io%i:",i,pwOut_save[i]);
//					serial_output("imx%i:imy%i:imz%i:lex%i:ley%i:lez%i:fgs%X:ofc%i:tx%i:ty%i:tz%i:G%d,%d,%d:AF%i,%i,%i:",IMUlocationx._.W1,IMUlocationy._.W1,IMUlocationz._.W1,
//					    locationErrorEarth[0], locationErrorEarth[1], locationErrorEarth[2],
//					    state_flags.WW, osc_fail_count,
//					    IMUvelocityx._.W1, IMUvelocityy._.W1, IMUvelocityz._.W1, goal.x, goal.y, goal.z, aero_force[0], aero_force[1], aero_force[2]);
//#if (USE_BAROMETER_ALTITUDE == 1)
//					serial_output("tmp%i:prs%li:alt%li:",
//					    get_barometer_temperature(), get_barometer_pressure(), 
//					    get_barometer_altitude());
//#endif
//					serial_output("bmv%i:mA%i:mAh%i:",
//#if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED)
//	                battery_voltage._.W1,
//#else
//	                (int16_t)0,
//#endif
//#if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED)                        
//					battery_current._.W1, battery_mAh_used._.W1);
//#else
//					(int16_t)0, (int16_t)0);                    
//#endif
//					serial_output("DH%i:",desiredHeight);
#if (RECORD_FREE_STACK_SPACE == 1)
					extern uint16_t maxstack;
					stack_free = (int16_t)(4096-maxstack); // This is actually wrong for the UDB4, but currently left the same as for telemetry.c
#endif // (RECORD_FREE_STACK_SPACE == 1)
//					serial_output("stk%d:", (int16_t)(4096-maxstack));
//					serial_output("\r\n");

					mavlink_msg_serial_udb_extra_f2_b_send(MAVLINK_COMM_0,
						tow.WW,
						pwIn_save[1], pwIn_save[2], pwIn_save[3], pwIn_save[4], pwIn_save[5],pwIn_save[6],
						pwIn_save[7], pwIn_save[8], pwIn_save[9], pwIn_save[10], pwIn_save[11], pwIn_save[12],
						pwOut_save[1], pwOut_save[2], pwOut_save[3], pwOut_save[4], pwOut_save[5], pwOut_save[6],
						pwOut_save[7], pwOut_save[8], pwOut_save[9], pwOut_save[10], pwOut_save[11], pwOut_save[12],
						IMUlocationx._.W1, IMUlocationy._.W1, IMUlocationz._.W1,
						locationErrorEarth[0], locationErrorEarth[1], locationErrorEarth[2],
						state_flags.WW,
#if (SILSIM != 1)
						osc_fail_count,
#else
						0,
#endif // (SILSIM != 1)
						IMUvelocityx._.W1, IMUvelocityy._.W1, IMUvelocityz._.W1,
						goal.x, goal.y, goal.z,
						aero_force[0], aero_force[1], aero_force[2],
#if (USE_BAROMETER_ALTITUDE == 1)
						get_barometer_temperature(), get_barometer_pressure(), 
						get_barometer_altitude(),
#else
						(int16_t)0, (int16_t)0, (int16_t)0,
#endif					
#if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED)
				                battery_voltage._.W1,
#else
				                (int16_t)0,
#endif
#if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED)                        
						battery_current._.W1, battery_mAh_used._.W1,
#else
						(int16_t)0, (int16_t)0,                   
#endif
						desiredHeight,
						stack_free);
						
				}
			}
			if (state_flags._.f13_print_req == 1)
			{
				// The F13 line of telemetry is printed when origin has been captured and in between F2 lines in SERIAL_UDB_EXTRA
				if (!f13_print_prepare)
				{
					return;
				}
				else
				{
					f13_print_prepare = false;
				}
//				serial_output("F13:week%i:origN%li:origE%li:origA%li:\r\n", week_no, lat_origin.WW, lon_origin.WW, alt_origin);
				mavlink_msg_serial_udb_extra_f13_send(MAVLINK_COMM_0, 
					week_no.BB, lat_origin.WW, lon_origin.WW, alt_origin.WW);
				
//				serial_output("F20:NUM_IN=%i:TRIM=",NUM_INPUTS);
				mavlink_msg_serial_udb_extra_f20_send(MAVLINK_COMM_0, 
					NUM_INPUTS,										\
					pwTrim_save[1],pwTrim_save[2],pwTrim_save[3],pwTrim_save[4],		\
					pwTrim_save[5],pwTrim_save[61],pwTrim_save[7],pwTrim_save[8],		\
					pwTrim_save[9],pwTrim_save[10],pwTrim_save[11],pwTrim_save[12] );
//				for (i = 1; i <= NUM_INPUTS; i++)
//				{
//					serial_output("%i,",udb_pwTrim[i]);
//				}
//				serial_output(":\r\n");
				state_flags._.f13_print_req = 0;
			}
			break;
		}
	}
	if (mavlink_sue_telemetry_counter)
	{
		mavlink_sue_telemetry_counter--;
	}
#if (USE_TELELOG == 1)
	log_swapbuf();
#endif
}