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);
}
}
}
}
}
int main()
{
// when working with the watchdog timer, it is important that your
// program's first action is to clear the MCUSR register
// (this is accomplished by calling clear_reset_flags()) and disable
// the watchdog timer. If the watchdog timer ever resets your program,
// the watchdog timer defaults to enabled with the shortest possible
// period (15 ms), which can lead to repeated resets if your program
// does not initially clear MCUSR and disable the watchdog timer.
unsigned char rf = get_reset_flags();
clear_reset_flags();
wdt_disable();
clear();
print("boot");
unsigned char i;
for (i = 0; i < 4; i++)
{
delay_ms(150);
print(".");
}
delay_ms(300);
wdt_enable(WDTO_2S); // enable watchdog timer with 2 sec period
while (1)
{
clear(); // clear LCD
// display last reset source on the LCD
if (rf & WATCHDOG_RESET)
print("WATCHDOG");
else if (rf & BROWNOUT_RESET)
print("BROWNOUT");
else if (rf & EXTERNAL_RESET)
print("EXTERNAL");
else if (rf & POWERON_RESET)
print("POWER-ON ");
else
print("none");
while (1)
{
lcd_goto_xy(0, 1); // go to start of second LCD row
print(" BUTTON ");
wdt_reset(); // reset the watchdog timer
unsigned long time = get_ms();
while (button_is_pressed(ANY_BUTTON))
{
// We loop here while button is held down and DO NOT
// reset the watchdog timer.
// Display a four-second countdown until WDT resets us.
int wdt_ms = 2000 - (int)(get_ms() - time);
lcd_goto_xy(0, 1); // go to start of second LCD row
print("WD ");
if (wdt_ms < 0)
print("0.00s");
else
{
print_long(wdt_ms / 1000); // seconds
print(".");
print_long((wdt_ms/100)%10); // tenths of seconds
print_long((wdt_ms/10)%10); // hundredths of seconds
print("s");
}
}
}
}
}
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 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(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
}