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);
}
}
}
}
}
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 ;
}
void cmd_cpuload(void)
{
printf("CPU Load %u%%\r\n", udb_cpu_load());
}
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 ;
}
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
}
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);
}
}
}
}
}
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
}