// Copies the serial_slave_buffer to the master and sends the
// serial_master_buffer to the slave.
//
// Returns:
// 0 => no error
// 1 => slave did not respond
int serial_update_buffers(void) {
// this code is very time dependent, so we need to disable interrupts
cli();
// signal to the slave that we want to start a transaction
serial_output();
serial_low();
_delay_us(1);
// wait for the slaves response
serial_input();
serial_high();
_delay_us(SERIAL_DELAY);
// check if the slave is present
if (serial_read_pin()) {
// slave failed to pull the line low, assume not present
sei();
return 1;
}
// if the slave is present syncronize with it
sync_recv();
uint8_t checksum_computed = 0;
// receive data from the slave
for (int i = 0; i < SERIAL_SLAVE_BUFFER_LENGTH; ++i) {
serial_slave_buffer[i] = serial_read_byte();
sync_recv();
checksum_computed += serial_slave_buffer[i];
}
uint8_t checksum_received = serial_read_byte();
sync_recv();
if (checksum_computed != checksum_received) {
sei();
return 1;
}
uint8_t checksum = 0;
// send data to the slave
for (int i = 0; i < SERIAL_MASTER_BUFFER_LENGTH; ++i) {
serial_write_byte(serial_master_buffer[i]);
sync_recv();
checksum += serial_master_buffer[i];
}
serial_write_byte(checksum);
sync_recv();
// always, release the line when not in use
serial_output();
serial_high();
sei();
return 0;
}
void mavlink_output_40hz(void)
#if (MAVLINK_TEST_ENCODE_DECODE == 1)
{
if (mavlink_test_first_pass_flag == 1)
{
serial_output("\r\nRunning MAVLink encode / decode Tests.\r\n");
// reset serial buffer in preparation for testing against buffer
mavlink_tests_pass = 0;
mavlink_tests_fail = 0;
mavlink_test_all(mavlink_system.sysid, mavlink_system.compid, &last_msg);
serial_output("\r\nMAVLink Tests Pass: %d\r\nMAVLink Tests Fail: %d\r\n", mavlink_tests_pass, mavlink_tests_fail);
mavlink_test_first_pass_flag = 0;
}
}
inline static
void change_reciver2sender(void) {
sync_recv(); //0
serial_delay(); //1
serial_low(); //3
serial_output(); //3
serial_delay_half1(); //4
}
void serial_output_8hz(void)
{
serial_output("lat: %li, long: %li, alt: %li\r\nrmat: %i, %i, %i, %i, %i, %i, %i, %i, %i\r\n",
lat_gps.WW, lon_gps.WW, alt_sl_gps.WW,
rmat[0], rmat[1], rmat[2],
rmat[3], rmat[4], rmat[5],
rmat[6], rmat[7], rmat[8]);
}
// Used by the slave to send a synchronization signal to the master.
static
void sync_send(void) {
serial_output();
serial_low();
serial_delay();
serial_high();
}
// Sends a byte with MSB ordering
static
void serial_write_byte(uint8_t data) {
uint8_t b = 8;
serial_output();
while( b-- ) {
if(data & (1 << b)) {
serial_high();
} else {
serial_low();
}
serial_delay();
}
}
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 )
{
unsigned int mode ;
struct relative2D matrix_accum ;
union longbbbb accum ;
int desired_dir_deg ; // desired_dir converted to a bearing (0-360)
long earth_pitch ; // pitch in binary angles ( 0-255 is 360 degreres)
long earth_roll ; // roll of the plane with respect to earth frame
//long earth_yaw ; // yaw with respect to earth frame
accum.WW = ( desired_dir * BYTECIR_TO_DEGREE ) + 32768 ;
desired_dir_deg = accum._.W1 - 90 ; // "Convert UAV DevBoad Earth" to Compass Bearing
if ( desired_dir_deg < 0 ) desired_dir_deg += 360 ;
if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 0)
mode = 1 ;
else if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 1)
mode = 2 ;
else if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 1)
mode = 3 ;
else if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 0)
mode = 0 ;
else
mode = 99 ; // Unknown
// Roll
// Earth Frame of Reference
matrix_accum.x = rmat[8] ;
matrix_accum.y = rmat[6] ;
earth_roll = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_roll = (-earth_roll * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// Pitch
// Earth Frame of Reference
// Note that we are using the matrix_accum.x
// left over from previous rect_to_polar in this calculation.
// so this Pitch calculation must follow the Roll calculation
matrix_accum.y = rmat[7] ;
earth_pitch = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_pitch = (-earth_pitch * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// Yaw
// Earth Frame of Reference
// Ardustation does not use yaw in degrees
// matrix_accum.x = rmat[4] ;
// matrix_accum.y = rmat[1] ;
// earth_yaw = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
// earth_yaw = (earth_yaw * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// The Ardupilot GroundStation protocol is mostly documented here:
// http://diydrones.com/profiles/blogs/ardupilot-telemetry-protocol
if (udb_heartbeat_counter % 40 == 0) // Every 8 runs (5 heartbeat counts per 8Hz)
{
serial_output("!!!LAT:%li,LON:%li,SPD:%.2f,CRT:%.2f,ALT:%li,ALH:%i,CRS:%.2f,BER:%i,WPN:%i,DST:%i,BTV:%.2f***\r\n"
"+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",
lat_gps.WW / 10 , long_gps.WW / 10 , (float)(sog_gps.BB / 100.0), (float)(climb_gps.BB / 100.0),
(alt_sl_gps.WW - alt_origin.WW) / 100, desiredHeight, (float)(cog_gps.BB / 100.0), desired_dir_deg,
waypointIndex, tofinish_line, (float)(voltage_milis.BB / 100.0),
(int)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),
earth_roll, earth_pitch,
mode
) ;
}
else if (udb_heartbeat_counter % 10 == 0) // Every 2 runs (5 heartbeat counts per 8Hz)
{
serial_output("+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",
(int)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),
earth_roll, earth_pitch,
mode
) ;
}
return ;
}
int soft_serial_transaction(void) {
SSTD_t *trans = Transaction_table;
#else
int soft_serial_transaction(int sstd_index) {
SSTD_t *trans = &Transaction_table[sstd_index];
#endif
cli();
// signal to the target that we want to start a transaction
serial_output();
serial_low();
_delay_us(SLAVE_INT_WIDTH_US);
#ifndef SERIAL_USE_MULTI_TRANSACTION
// wait for the target response
serial_input_with_pullup();
_delay_us(SLAVE_INT_RESPONSE_TIME);
// check if the target is present
if (serial_read_pin()) {
// target failed to pull the line low, assume not present
serial_output();
serial_high();
*trans->status = TRANSACTION_NO_RESPONSE;
sei();
return TRANSACTION_NO_RESPONSE;
}
#else
// send transaction table index
sync_send();
_delay_sub_us(TID_SEND_ADJUST);
serial_write_chunk(sstd_index, 4);
serial_delay_half1();
// wait for the target response (step1 low->high)
serial_input_with_pullup();
while( !serial_read_pin() ) {
_delay_sub_us(2);
}
// check if the target is present (step2 high->low)
for( int i = 0; serial_read_pin(); i++ ) {
if (i > SLAVE_INT_ACK_WIDTH + 1) {
// slave failed to pull the line low, assume not present
serial_output();
serial_high();
*trans->status = TRANSACTION_NO_RESPONSE;
sei();
return TRANSACTION_NO_RESPONSE;
}
_delay_sub_us(SLAVE_INT_ACK_WIDTH_UNIT);
}
#endif
// initiator recive phase
// if the target is present syncronize with it
if( trans->target2initiator_buffer_size > 0 ) {
if (!serial_recive_packet((uint8_t *)trans->target2initiator_buffer,
trans->target2initiator_buffer_size) ) {
serial_output();
serial_high();
*trans->status = TRANSACTION_DATA_ERROR;
sei();
return TRANSACTION_DATA_ERROR;
}
}
// initiator switch to output
change_reciver2sender();
// initiator send phase
if( trans->initiator2target_buffer_size > 0 ) {
serial_send_packet((uint8_t *)trans->initiator2target_buffer,
trans->initiator2target_buffer_size);
}
// always, release the line when not in use
sync_send();
*trans->status = TRANSACTION_END;
sei();
return TRANSACTION_END;
}
void soft_serial_initiator_init(SSTD_t *sstd_table)
{
Transaction_table = sstd_table;
serial_output();
serial_high();
}
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 serial_master_init(void) {
serial_output();
serial_high();
}
