/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
FlightTelemetryStatsData flightStats;
int32_t retries;
int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Only process event if connected to GCS or if object FlightTelemetryStats is updated
FlightTelemetryStatsGet(&flightStats);
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// Act on event
retries = 0;
success = -1;
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
}
// Update stats
txRetries += (retries - 1);
if (success == -1) {
++txErrors;
}
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
// Update stats
txRetries += (retries - 1);
if (success == -1) {
++txErrors;
}
}
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
}
}
}
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
// FlightTelemetryStatsData flightStats;
// GCSTelemetryStatsData gcsTelemetryStatsData;
// int32_t retries;
// int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
}
mavlink_message_t msg;
mavlink_system.sysid = 20;
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.type = MAV_TYPE_FIXED_WING;
uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT;
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
// Setup type and object id fields
uint32_t objId = UAVObjGetID(ev->obj);
// uint64_t timeStamp = 0;
switch(objId) {
case BAROALTITUDE_OBJID:
{
BaroAltitudeGet(&baroAltitude);
pressure.press_abs = baroAltitude.Pressure*10.0f;
pressure.temperature = baroAltitude.Temperature*100.0f;
mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case FLIGHTTELEMETRYSTATS_OBJID:
{
// FlightTelemetryStatsData flightTelemetryStats;
FlightTelemetryStatsGet(&flightStats);
// XXX this is a hack to make it think it got a confirmed
// connection
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
GCSTelemetryStatsGet(&gcsTelemetryStatsData);
gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
//
//
// //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass);
// mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case SYSTEMSTATS_OBJID:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t system_state = MAV_STATE_UNINIT;
uint8_t base_mode = 0;
uint8_t custom_mode = 0;
// Set flight mode
switch (flightStatus.FlightMode)
{
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
default:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
}
// Set arming state
switch (flightStatus.Armed)
{
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
system_state = MAV_STATE_ACTIVE;
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case FLIGHTSTATUS_ARMED_DISARMED:
system_state = MAV_STATE_STANDBY;
base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED;
break;
}
// Set HIL
if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state);
SystemStatsData stats;
SystemStatsGet(&stats);
FlightBatteryStateData flightBatteryData;
FlightBatteryStateGet(&flightBatteryData);
FlightBatterySettingsData flightBatterySettings;
FlightBatterySettingsGet(&flightBatterySettings);
uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f);
int16_t batteryCurrent = -1; // -1: Not present / not estimated
int8_t batteryPercent = -1; // -1: Not present / not estimated
// if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0)
// {
// Factor is zero, sensor is not present
// Estimate remaining capacity based on lipo curve
batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f));
// }
// else
// {
// // Use capacity and current
// batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity);
// batteryCurrent = flightBatteryData.Current*100;
// }
mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case ATTITUDERAW_OBJID:
{
AttitudeRawGet(&attitudeRaw);
// Copy data
attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X];
attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y];
attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z];
attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X];
attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y];
attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z];
attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X];
attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y];
attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z];
mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
if (hilEnabled)
{
mavlink_hil_controls_t controls;
// Copy data
controls.roll_ailerons = 0.1;
controls.pitch_elevator = 0.1;
controls.yaw_rudder = 0.0;
controls.throttle = 0.8;
mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
}
case ATTITUDEMATRIX_OBJID:
{
AttitudeMatrixGet(&attitudeMatrix);
// Copy data
attitude.roll = attitudeMatrix.Roll;
attitude.pitch = attitudeMatrix.Pitch;
attitude.yaw = attitudeMatrix.Yaw;
attitude.rollspeed = attitudeMatrix.AngularRates[0];
attitude.pitchspeed = attitudeMatrix.AngularRates[1];
attitude.yawspeed = attitudeMatrix.AngularRates[2];
mavlink_msg_attitude_encode(mavlink_system.sysid,
mavlink_system.compid, &msg, &attitude);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case GPSPOSITION_OBJID:
{
GPSPositionGet(&gpsPosition);
gps_raw.time_usec = 0;
gps_raw.lat = gpsPosition.Latitude*10;
gps_raw.lon = gpsPosition.Longitude*10;
gps_raw.alt = gpsPosition.Altitude*10;
gps_raw.eph = gpsPosition.HDOP*100;
gps_raw.epv = gpsPosition.VDOP*100;
gps_raw.cog = gpsPosition.Heading*100;
gps_raw.satellites_visible = gpsPosition.Satellites;
gps_raw.fix_type = gpsPosition.Status;
mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
// mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0);
break;
}
case POSITIONACTUAL_OBJID:
{
PositionActualData pos;
PositionActualGet(&pos);
mavlink_local_position_ned_t m_pos;
m_pos.time_boot_ms = 0;
m_pos.x = pos.North;
m_pos.y = pos.East;
m_pos.z = pos.Down;
m_pos.vx = 0.0f;
m_pos.vy = 0.0f;
m_pos.vz = 0.0f;
mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
case ACTUATORCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
ActuatorCommandData act;
ActuatorCommandGet(&act);
rc.chan5_scaled = act.Channel[0];
rc.chan6_scaled = act.Channel[1];
rc.chan7_scaled = act.Channel[2];
rc.chan8_scaled = act.Channel[3];
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
case MANUALCONTROLCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
rc.chan5_scaled = 0;
rc.chan6_scaled = 0;
rc.chan7_scaled = 0;
rc.chan8_scaled = 0;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
default:
{
//printf("unknown object: %x\n",(unsigned int)objId);
break;
}
}
}
}