/** * Execute the requested transaction on an object. * \param[in] connection UAVTalkConnection to be used * \param[in] type Transaction type * UAVTALK_TYPE_OBJ: send object, * UAVTALK_TYPE_OBJ_REQ: request object update * UAVTALK_TYPE_OBJ_ACK: send object with an ack * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately * \return 0 Success * \return -1 Failure */ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs) { int32_t respReceived; int32_t ret = -1; // Send object depending on if a response is needed if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); // Send object xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // expected response type connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; connection->respObjId = UAVObjGetID(obj); connection->respInstId = instId; ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); // Wait for response (or timeout) if sending the object succeeded respReceived = pdFALSE; if (ret == 0) { respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); } // Check if a response was received if (respReceived == pdTRUE) { // We are done successfully xSemaphoreGiveRecursive(connection->transLock); ret = 0; } else { // Cancel transaction xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // non blocking call to make sure the value is reset to zero (binary sema) xSemaphoreTake(connection->respSema, 0); connection->respObjId = 0; xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->transLock); return -1; } } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) { xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); } return ret; }
/** * @brief Load one object instance per sector * @param[in] obj UAVObjHandle the object to save * @param[in] instId The instance of the object to save * @return 0 if success or error code * @retval -1 if object not in file table * @retval -2 if unable to retrieve object header * @retval -3 if loaded data instId or objId don't match * @retval -4 if unable to retrieve instance data * @retval -5 if unable to read CRC * @retval -6 if CRC doesn't match * @note This uses one sector on the flash chip per object so that no buffering in ram * must be done when erasing the sector before a save */ int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data) { uint32_t objId = UAVObjGetID(obj); uint16_t objSize = UAVObjGetNumBytes(obj); uint8_t crc = 0; uint8_t crcFlash = 0; const uint8_t crc_read_step = 8; uint8_t crc_read_buffer[crc_read_step]; int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); // Object currently not saved if(addr < 0) return -1; struct fileHeader header; // Load header // This information IS redundant with the object table id. Oh well. Better safe than sorry. if(PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0) return -2; // Update CRC crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header)); if((header.id != objId) || (header.instId != instId)) return -3; // To avoid having to allocate the RAM for a copy of the object, we read by chunks // and compute the CRC for(uint32_t i = 0; i < objSize; i += crc_read_step) { PIOS_Flash_W25X_ReadData(addr + sizeof(header) + i, crc_read_buffer, crc_read_step); uint8_t valid_bytes = ((i + crc_read_step) >= objSize) ? objSize - i : crc_read_step; crc = PIOS_CRC_updateCRC(crc, crc_read_buffer, valid_bytes); } // Read CRC (written so will work when CRC changes to uint16) if(PIOS_Flash_W25X_ReadData(addr + sizeof(header) + objSize, (uint8_t *) &crcFlash, sizeof(crcFlash)) != 0) return -5; if(crc != crcFlash) return -6; // Read the instance data if (PIOS_Flash_W25X_ReadData(addr + sizeof(header), data, objSize) != 0) return -4; return 0; }
/** * @brief Delete object from flash * @param[in] obj UAVObjHandle the object to save * @param[in] instId The instance of the object to save * @return 0 if success or error code * @retval -1 if object not in file table * @retval -2 Erase failed * @note To avoid buffering the file table (1k ram!) the entry in the file table * remains but destination sector is erased. This will make the load fail as the * file header won't match the object. At next save it goes back there. */ int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId) { uint32_t objId = UAVObjGetID(obj); int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); // Object currently not saved if(addr < 0) return -1; if(PIOS_Flash_W25X_EraseSector(addr) != 0) return -2; return 0; }
/** * Execute the requested transaction on an object. * \param[in] connection UAVLinkConnection to be used * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type * UAVLINK_TYPE_OBJ: send object, * UAVLINK_TYPE_OBJ_REQ: request object update * UAVLINK_TYPE_OBJ_ACK: send object with an ack * \return 0 Success * \return -1 Failure */ static int32_t objectTransaction(UAVLinkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) { int32_t respReceived; // Send object depending on if a response is needed if (type == UAVLINK_TYPE_OBJ_ACK || type == UAVLINK_TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); // Send object xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); connection->respId = UAVObjGetID(obj); connection->respInstId = instId; sendObject(connection, obj, instId, type); xSemaphoreGiveRecursive(connection->lock); // Wait for response (or timeout) respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS); // Check if a response was received if (respReceived == pdFALSE) { // Cancel transaction xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) connection->respId = 0; xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->transLock); return -1; } else { xSemaphoreGiveRecursive(connection->transLock); return 0; } } else if (type == UAVLINK_TYPE_OBJ) { xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); sendObject(connection, obj, instId, UAVLINK_TYPE_OBJ); xSemaphoreGiveRecursive(connection->lock); return 0; } else { return -1; } }
/** * @brief Saves one object instance per sector * @param[in] obj UAVObjHandle the object to save * @param[in] instId The instance of the object to save * @return 0 if success or -1 if failure * @note This uses one sector on the flash chip per object so that no buffering in ram * must be done when erasing the sector before a save */ int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data) { uint32_t objId = UAVObjGetID(obj); uint8_t crc = 0; int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); // Object currently not saved if(addr < 0) addr = PIOS_FLASHFS_GetNewAddress(objId, instId); // Could not allocate a sector if(addr < 0) return -1; struct fileHeader header = { .id = objId, .instId = instId, .size = UAVObjGetNumBytes(obj) }; if(PIOS_Flash_W25X_EraseSector(addr) != 0) return -2; // Save header // This information IS redundant with the object table id. Oh well. Better safe than sorry. if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0) return -3; // Update CRC crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header)); // Save data if(PIOS_Flash_W25X_WriteData(addr + sizeof(header), data, UAVObjGetNumBytes(obj)) != 0) return -4; // Update CRC crc = PIOS_CRC_updateCRC(crc, (uint8_t *) data, UAVObjGetNumBytes(obj)); // Save CRC (written so will work when CRC changes to uint16) if(PIOS_Flash_W25X_WriteData(addr + sizeof(header) + UAVObjGetNumBytes(obj), (uint8_t *) &crc, sizeof(crc)) != 0) return -4; return 0; }
/** * 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; } } } }