diff --git a/telemetry/telemetry.c b/telemetry/telemetry.c index 7ee033a..ba98edb 100644 --- a/telemetry/telemetry.c +++ b/telemetry/telemetry.c @@ -54,6 +54,12 @@ static void telemetry_build_msg_vehicle_id ); +static void telemetry_build_msg_calibration + ( + TELEMETRY_MESSAGE* msg_buf + ); + + static void telemetry_build_msg_dashboard_dump ( TELEMETRY_MESSAGE* msg_buf @@ -85,11 +91,16 @@ void telemetry_get_next_message TELEMETRY_MESSAGE_TYPES msg_type; /* Determine which payload to send */ -if( ( message_idx % 2 == 0 ) +if( ( message_idx % 3 == 0 ) && ( get_fc_state() == FC_STATE_LAUNCH_DETECT ) ) { msg_type = TELEMETRY_MSG_VEHICLE_ID; } +else if( ( message_idx % 3 == 1 ) + && ( get_fc_state() == FC_STATE_LAUNCH_DETECT ) ) + { + msg_type = TELEMETRY_MSG_CALIBRATION; + } else { msg_type = TELEMETRY_MSG_DASHBOARD_DATA; @@ -137,6 +148,11 @@ switch( message_type ) telemetry_build_msg_dashboard_dump(msg_buf); break; } + case TELEMETRY_MSG_CALIBRATION: + { + telemetry_build_msg_calibration(msg_buf); + break; + } default: { error_fail_fast( ERROR_RECORD_FLIGHT_EVENTS_ERROR ); @@ -195,6 +211,46 @@ strncpy( msg_buf->payload.vehicle_id.flight_id, "AVIONICS_TEST", 16 ); } /* telemetry_build_msg_vehicle_id */ +/********************************************************************************* +* * +* FUNCTION: * +* telemetry_build_msg_calibration * +* * +* DESCRIPTION: * +* Build the calibration payload. Assume header is filled by caller. * +* * +*********************************************************************************/ +static void telemetry_build_msg_calibration + ( + TELEMETRY_MESSAGE* msg_buf + ) +{ +/*------------------------------------------------------------------------------ + Local Variables +------------------------------------------------------------------------------*/ +SENSOR_DATA qfe_sensor_data; + +memset( &qfe_sensor_data, 0, sizeof( SENSOR_DATA ) ); + +/*------------------------------------------------------------------------------ + Construct known elements from preset data +------------------------------------------------------------------------------*/ +msg_buf->payload.calibration.imu_offset = preset_data.imu_offset; +msg_buf->payload.calibration.baro_preset = preset_data.baro_preset; +msg_buf->payload.calibration.servo_preset = preset_data.servo_preset; + +/*------------------------------------------------------------------------------ + Determine QFE reference elevation +------------------------------------------------------------------------------*/ +qfe_sensor_data.baro_pressure = preset_data.baro_preset.baro_pres; +qfe_sensor_data.baro_temp = preset_data.baro_preset.baro_temp; +sensor_baro_alt( &qfe_sensor_data ); + +msg_buf->payload.calibration.qfe_elevation = qfe_sensor_data.baro_alt; + +} /* telemetry_build_msg_calibration */ + + /********************************************************************************* * * * FUNCTION: * diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index 38b1959..b6f5724 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -75,6 +75,7 @@ typedef enum _TELEMETRY_MESSAGE_TYPES { TELEMETRY_MSG_VEHICLE_ID = 0x00000001, TELEMETRY_MSG_DASHBOARD_DATA = 0x00000002, + TELEMETRY_MSG_CALIBRATION = 0x00000003, __TELEMETRY_MSG_FORCE_32BIT = 0xFFFFFFFF /* used to force this type size to 32 bits */ } TELEMETRY_MESSAGE_TYPES; _Static_assert( sizeof(TELEMETRY_MESSAGE_TYPES) == 4, "TELEMETRY_MESSAGE_TYPES size invalid."); @@ -115,6 +116,15 @@ typedef struct __attribute__((packed)) _TELEMETRY_MSG_DASHBOARD_DUMP_TYPE } TELEMETRY_MSG_DASHBOARD_DUMP_TYPE; _Static_assert( sizeof(TELEMETRY_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_DASHBOARD_DUMP_TYPE size invalid."); +typedef struct __attribute__((packed)) _TELEMETRY_MSG_CALIBRATION_TYPE + { + IMU_OFFSET imu_offset; + BARO_PRESET baro_preset; + float qfe_elevation; + SERVO_PRESET servo_preset; + } TELEMETRY_MSG_CALIBRATION_TYPE; + _Static_assert( sizeof(TELEMETRY_MSG_CALIBRATION_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_CALIBRATION size invalid." ); + /* struct is packed to inhibit padding */ typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE { @@ -123,6 +133,7 @@ typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE { TELEMETRY_MSG_VEHICLE_ID_TYPE vehicle_id; /* provide information about the transmitting device */ TELEMETRY_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; /* gives vehicle state info (location, orientation) */ + TELEMETRY_MSG_CALIBRATION_TYPE calibration; } payload; } TELEMETRY_MESSAGE; _Static_assert( sizeof(TELEMETRY_MESSAGE) == TELEMETRY_MESSAGE_SIZE, "LORA_PAYLOAD size invalid.");