static enum opahrs_result opahrs_msg_v1_send_req(const struct opahrs_msg_v1 *req)
{
int32_t rc;
struct opahrs_msg_v1 link_rx;
for (uint8_t retries = 0; retries < 20; retries++) {
struct opahrs_msg_v1 *rsp = &link_rx;
if ((rc = opahrs_msg_txrx((const uint8_t *)req, (uint8_t *) rsp, sizeof(*rsp))) < 0) {
return OPAHRS_RESULT_FAILED;
}
/* Make sure we got a sane response by checking the magic */
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) || (rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
return OPAHRS_RESULT_FAILED;
}
switch (rsp->head.type) {
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
case OPAHRS_MSG_LINK_STATE_INACTIVE:
/* Wait for a small delay and retry */
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
continue;
case OPAHRS_MSG_LINK_STATE_READY:
/* Peer was ready when we Tx'd so they have now Rx'd our message */
return OPAHRS_RESULT_OK;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
/* Wait for a small delay and retry */
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(50);
#endif
continue;
}
}
return OPAHRS_RESULT_TIMEOUT;
}
/***********************************************************************
* FUNCTION: LEDOk
* DESCRIPTION: let the white led be an acknowledge:
* 600 ms permanent ON
* PARAMETER: -
* RETURN: -
* COMMENT: #ifdef HW_LEDRYW: - uses white LED for BMW version
* #else - uses lower led in green mode
*********************************************************************** */
void LEDOk(void)
{
MESSAGE msg;
#ifdef HW_LEDRYW /* WE USE WHITE OK LED */
LED_MSG_MS(msg, LED_FLASH, LED_ON, 1, 0); /* set led ON now! */
MsgQPostMsg(msg, MSGQ_PRIO_LOW);
LED_MSG_MS(msg, LED_FLASH, LED_OFF, 0, 1); /* switch off led after 600 ms */
SetTimerMsg(msg, MS2TICKS(600));
#else /* WE USE GREEN DUAL LED */
LED_MSG_MS(msg, LED1, LED_GREEN, 1, 0); /* set led ON now! */
MsgQPostMsg(msg, MSGQ_PRIO_LOW);
LED_MSG_MS(msg, LED1, LED_OFF, 0, 1); /* switch off led after 600 ms */
SetTimerMsg(msg, MS2TICKS(600));
#endif
}
/**
*
* @brief Suspends execution of current thread at least for specified time.
*
* @param[in] time_ms time in milliseconds to suspend thread execution
*
*/
void PIOS_Thread_Sleep(uint32_t time_ms)
{
if (time_ms == PIOS_THREAD_TIMEOUT_MAX)
vTaskDelay(portMAX_DELAY);
else
vTaskDelay((portTickType)MS2TICKS(time_ms));
}
/***********************************************************************
* FUNCTION: LEDEsc
* DESCRIPTION: let the red led be an error indicator:
* 1000 ms duration pulsed with 100 ms ON / 100 ms OFF
* PARAMETER: -
* RETURN: -
* COMMENT: #ifdef HW_LEDRYW: - uses red LED for BMW version
* #else - uses lower led in red mode
*********************************************************************** */
void LEDEsc(void)
{
MESSAGE msg;
#ifdef HW_LEDRYW /* WE USE RED ESC LED */
LED_MSG_MS(msg, LED_OIL, LED_ON, 100, 100); /* set led ON now! */
MsgQPostMsg(msg, MSGQ_PRIO_LOW);
LED_MSG_MS(msg, LED_OIL, LED_OFF, 0, 1); /* switch off led after 1000 ms */
SetTimerMsg(msg, MS2TICKS(1000));
#else /* WE USE RED DUAL LED */
LED_MSG_MS(msg, LED1, LED_RED, 100, 100); /* set led ON now! */
MsgQPostMsg(msg, MSGQ_PRIO_LOW);
LED_MSG_MS(msg, LED1, LED_OFF, 0, 1); /* switch off led after 1000 ms */
SetTimerMsg(msg, MS2TICKS(1000));
#endif
}
/**
*
* @brief Takes binary semaphore.
*
* @param[in] sema pointer to instance of @p struct pios_semaphore
* @param[in] timeout_ms timeout for acquiring the lock in milliseconds
*
* @returns true on success or false on timeout or failure
*
*/
bool PIOS_Semaphore_Take(struct pios_semaphore *sema, uint32_t timeout_ms)
{
PIOS_Assert(sema != NULL);
#if defined(PIOS_INCLUDE_FREERTOS)
portTickType timeout_ticks;
if (timeout_ms == PIOS_SEMAPHORE_TIMEOUT_MAX)
timeout_ticks = portMAX_DELAY;
else
timeout_ticks = MS2TICKS(timeout_ms);
return xSemaphoreTake(sema->sema_handle, timeout_ticks) == pdTRUE;
#else
uint32_t start = PIOS_DELAY_GetRaw();
uint32_t temp_sema_count;
do {
PIOS_IRQ_Disable();
if ((temp_sema_count = sema->sema_count) != 0)
--sema->sema_count;
PIOS_IRQ_Enable();
} while (temp_sema_count == 0 &&
PIOS_DELAY_DiffuS(start) < timeout_ms * 1000);
return temp_sema_count != 0;
#endif
}
/* void sync(int): synchronize an interval by given ms-value */
void SystemSync(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
static portTickType lastSysTime;
if ((lastSysTime == 0) || (Param[0]->Val->Integer == 0)) {
lastSysTime = xTaskGetTickCount();
}
if (Param[0]->Val->Integer > 0) {
vTaskDelayUntil(&lastSysTime, MS2TICKS(Param[0]->Val->Integer));
}
}
/**
*
* @brief Locks a non recursive mutex.
*
* @param[in] mtx pointer to instance of @p struct pios_mutex
* @param[in] timeout_ms timeout for acquiring the lock in milliseconds
*
* @returns true on success or false on timeout or failure
*
*/
bool PIOS_Mutex_Lock(struct pios_mutex *mtx, uint32_t timeout_ms)
{
PIOS_Assert(mtx != NULL);
portTickType timeout_ticks;
if (timeout_ms == PIOS_MUTEX_TIMEOUT_MAX)
timeout_ticks = portMAX_DELAY;
else
timeout_ticks = MS2TICKS(timeout_ms);
return xSemaphoreTake(mtx->mtx_handle, timeout_ticks) == pdTRUE;
}
enum opahrs_result PIOS_OPAHRS_resync(void)
{
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
enum opahrs_result rc = OPAHRS_RESULT_FAILED;
opahrs_msg_v1_init_link_tx(&req, OPAHRS_MSG_LINK_TAG_NOP);
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
for (uint32_t i = 0; i < sizeof(req); i++) {
/* Tx a shortened (by one byte) message to walk through all byte positions */
opahrs_msg_v1_init_rx(&rsp);
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) & req, (uint8_t *) & rsp, sizeof(req) - 1, NULL);
/* Good magic means we're sync'd */
if ((rsp.head.magic == OPAHRS_MSG_MAGIC_HEAD) && (rsp.tail.magic == OPAHRS_MSG_MAGIC_TAIL)) {
/* We need to shift out one more byte to compensate for the short tx */
PIOS_SPI_TransferByte(PIOS_OPAHRS_SPI, 0x00);
rc = OPAHRS_RESULT_OK;
break;
}
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(10);
#endif
}
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
//vTaskDelay(MS2TICKS(5));
return rc;
}
static int32_t opahrs_msg_txrx(const uint8_t * tx, uint8_t * rx, uint32_t len)
{
int32_t rc;
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
rc = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, tx, rx, len, NULL);
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
return (rc);
}
static enum opahrs_result opahrs_msg_v1_recv_rsp(enum opahrs_msg_v1_tag tag, struct opahrs_msg_v1 *rsp)
{
struct opahrs_msg_v1 link_tx;
opahrs_msg_v1_init_link_tx(&link_tx, OPAHRS_MSG_LINK_TAG_NOP);
for (uint8_t retries = 0; retries < 20; retries++) {
if (opahrs_msg_txrx((const uint8_t *)&link_tx, (uint8_t *) rsp, sizeof(*rsp)) < 0) {
return OPAHRS_RESULT_FAILED;
}
/* Make sure we got a sane response by checking the magic */
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) || (rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
return OPAHRS_RESULT_FAILED;
}
switch (rsp->head.type) {
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
/* Wait for a small delay and retry */
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
continue;
case OPAHRS_MSG_LINK_STATE_INACTIVE:
case OPAHRS_MSG_LINK_STATE_READY:
/* somehow, we've missed our response */
return OPAHRS_RESULT_FAILED;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
/* This isn't the type we expected */
return OPAHRS_RESULT_FAILED;
break;
case OPAHRS_MSG_TYPE_USER_V1:
if (rsp->payload.user.t == tag) {
return OPAHRS_RESULT_OK;
} else {
return OPAHRS_RESULT_FAILED;
}
break;
}
}
return OPAHRS_RESULT_TIMEOUT;
}
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
{
ExampleSettingsData settings;
ExampleObject2Data data;
int32_t step;
portTickType lastSysTime;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
// Update settings with latest value
ExampleSettingsGet(&settings);
// Get the object data
ExampleObject2Get(&data);
// Determine how to update the data
if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) {
step = settings.StepSize;
} else {
step = -settings.StepSize;
}
// Update the data
data.Field1 += step;
data.Field2 += step;
data.Field3 += step;
data.Field4[0] += step;
data.Field4[1] += step;
// Update the ExampleObject, after this function is called
// notifications to any other modules listening to that object
// will be sent and the GCS object will be updated through the
// telemetry link. All operations will take place asynchronously
// and the following call will return immediately.
ExampleObject2Set(&data);
// Since this module executes at fixed time intervals, we need to
// block the task until it is time for the next update.
// The settings field is in ms, to convert to RTOS ticks we need
// to use the MS2TICKS macro.
vTaskDelayUntil(&lastSysTime, MS2TICKS(settings.UpdatePeriod));
}
}
/**
*
* @brief Suspends execution of current thread for a regular interval.
*
* @param[in] previous_ms pointer to system time of last execution,
* must have been initialized with PIOS_Thread_Systime() on first invocation
* @param[in] increment_ms time of regular interval in milliseconds
*
*/
void PIOS_Thread_Sleep_Until(uint32_t *previous_ms, uint32_t increment_ms)
{
portTickType temp = MS2TICKS(*previous_ms);
vTaskDelayUntil(&temp, (portTickType)MS2TICKS(increment_ms));
*previous_ms = TICKS2MS(temp);
}
static void gpsTask(void *parameters)
{
portTickType xDelay = MS2TICKS(GPS_COM_TIMEOUT_MS);
uint32_t timeNowMs = TICKS2MS(xTaskGetTickCount());
GPSPositionData gpsposition;
uint8_t gpsProtocol;
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
gps_airspeed_initialize();
#endif
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
#if !defined(PIOS_GPS_MINIMAL)
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
{
uint8_t gpsAutoConfigure;
ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);
if (gpsAutoConfigure == MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {
// Wait for power to stabilize before talking to external devices
vTaskDelay(MS2TICKS(1000));
// Runs through a number of possible GPS baud rates to
// configure the ublox baud rate. This uses a NMEA string
// so could work for either UBX or NMEA actually. This is
// somewhat redundant with updateSettings below, but that
// is only called on startup and is not an issue.
ModuleSettingsGPSSpeedOptions baud_rate;
ModuleSettingsGPSSpeedGet(&baud_rate);
ubx_cfg_set_baudrate(gpsPort, baud_rate);
vTaskDelay(MS2TICKS(1000));
ubx_cfg_send_configuration(gpsPort, gps_rx_buffer);
}
}
break;
#endif
}
#endif /* PIOS_GPS_MINIMAL */
GPSPositionGet(&gpsposition);
// Loop forever
while (1)
{
uint8_t c;
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
{
int res;
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
}
if (res == PARSER_COMPLETE) {
timeNowMs = TICKS2MS(xTaskGetTickCount());
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// Check for GPS timeout
timeNowMs = TICKS2MS(xTaskGetTickCount());
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
//Wait until our turn
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_MPXV));
//Ensure that the ADC pin is properly configured
if(airspeedADCPin <0){ //It's not, so revert to former sensor type
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV) { //First let sensor warm up and stabilize.
calibrationCount++;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) { //Then compute the average.
calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
uint16_t sensorCalibration;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV7002_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV5004_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
//Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV)
AirspeedSettingsZeroPointSet(&sensorCalibration);
return;
}
//Get CAS
float calibratedAirspeed=0;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
calibratedAirspeed = PIOS_MPXV7002_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV7002_Measure(airspeedADCPin);
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
calibratedAirspeed = PIOS_MPXV5004_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV5004_Measure(airspeedADCPin);
}
//Filter CAS
float alpha=SAMPLING_DELAY_MS_MPXV/(SAMPLING_DELAY_MS_MPXV + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CalibratedAirspeed*(1.0f-alpha);
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
}
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
/* Do nothing when driver support not compiled. */
vTaskDelayUntil(lastSysTime, MS2TICKS(1000));
}
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
portTickType lastSysTime;
uint16_t prev_tx_count = 0;
uint16_t prev_rx_count = 0;
bool first_time = true;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Main system loop
while (1) {
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
// Update the PipXstatus UAVO
OPLinkStatusData oplinkStatus;
uint32_t pairID;
OPLinkStatusGet(&oplinkStatus);
OPLinkSettingsPairIDGet(&pairID);
// Get the other device stats.
PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
// Get the stats from the radio device
struct rfm22b_stats radio_stats;
PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
// Update the status
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
oplinkStatus.RxGood = radio_stats.rx_good;
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
oplinkStatus.RxErrors = radio_stats.rx_error;
oplinkStatus.RxMissed = radio_stats.rx_missed;
oplinkStatus.RxFailure = radio_stats.rx_failure;
oplinkStatus.TxDropped = radio_stats.tx_dropped;
oplinkStatus.TxResent = radio_stats.tx_resent;
oplinkStatus.TxFailure = radio_stats.tx_failure;
oplinkStatus.Resets = radio_stats.resets;
oplinkStatus.Timeouts = radio_stats.timeouts;
oplinkStatus.RSSI = radio_stats.rssi;
oplinkStatus.AFCCorrection = radio_stats.afc_correction;
oplinkStatus.LinkQuality = radio_stats.link_quality;
if (first_time)
first_time = false;
else
{
uint16_t tx_count = radio_stats.tx_byte_count;
uint16_t rx_count = radio_stats.rx_byte_count;
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
prev_tx_count = tx_count;
prev_rx_count = rx_count;
}
oplinkStatus.TXSeq = radio_stats.tx_seq;
oplinkStatus.RXSeq = radio_stats.rx_seq;
oplinkStatus.LinkState = radio_stats.link_state;
if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
LINK_LED_ON;
else
LINK_LED_OFF;
// Update the object
OPLinkStatusSet(&oplinkStatus);
// Wait until next period
vTaskDelayUntil(&lastSysTime, MS2TICKS(SYSTEM_UPDATE_PERIOD_MS));
}
}
static void SensorsTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
// HomeLocationData homeLocation;
// HomeLocationGet(&homeLocation);
// homeLocation.Latitude = 0;
// homeLocation.Longitude = 0;
// homeLocation.Altitude = 0;
// homeLocation.Be[0] = 26000;
// homeLocation.Be[1] = 400;
// homeLocation.Be[2] = 40000;
// homeLocation.Set = HOMELOCATION_SET_TRUE;
// HomeLocationSet(&homeLocation);
PIOS_SENSORS_SetMaxGyro(500);
// Main task loop
while (1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
switch(systemSettings.AirframeType) {
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
sensor_sim_type = MODEL_AIRPLANE;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
sensor_sim_type = MODEL_QUADCOPTER;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
sensor_sim_type = MODEL_CAR;
break;
default:
sensor_sim_type = MODEL_AGNOSTIC;
}
static int i;
i++;
if (i % 5000 == 0) {
//float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
//fprintf(stderr, "Sensor relative timing: %f\n", dT);
}
sensors_count++;
switch(sensor_sim_type) {
case CONSTANT:
simulateConstant();
break;
case MODEL_AGNOSTIC:
simulateModelAgnostic();
break;
case MODEL_QUADCOPTER:
simulateModelQuadcopter();
break;
case MODEL_AIRPLANE:
simulateModelAirplane();
break;
case MODEL_CAR:
simulateModelCar();
}
vTaskDelay(MS2TICKS(2));
}
}
/* void delay(int): sleep for given ms-value */
void SystemDelay(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if (Param[0]->Val->Integer > 0) {
vTaskDelay(MS2TICKS(Param[0]->Val->Integer));
}
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
{
bool engaged = false;
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
AltitudeHoldSettingsData altitudeHoldSettings;
UAVObjEvent ev;
struct pid velocity_pid;
// Listen for object updates.
AltitudeHoldDesiredConnectQueue(queue);
AltitudeHoldSettingsConnectQueue(queue);
FlightStatusConnectQueue(queue);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);
// Main task loop
const uint32_t dt_ms = 5;
const float dt_s = dt_ms * 0.001f;
uint32_t timeout = dt_ms;
while (1) {
if ( xQueueReceive(queue, &ev, MS2TICKS(timeout)) != pdTRUE ) {
} else if (ev.obj == FlightStatusHandle()) {
uint8_t flight_mode;
FlightStatusFlightModeGet(&flight_mode);
if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator);
velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000
engaged = true;
} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
engaged = false;
// Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged
timeout = engaged ? dt_ms : 100;
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
} else if (ev.obj == AltitudeHoldSettingsHandle()) {
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
}
// When engaged compute altitude controller output
if (engaged) {
float position_z, velocity_z, altitude_error;
PositionActualDownGet(&position_z);
VelocityActualDownGet(&velocity_z);
position_z = -position_z; // Use positive up convention
velocity_z = -velocity_z; // Use positive up convention
// Compute the altitude error
altitude_error = altitudeHoldDesired.Altitude - position_z;
// Velocity desired is from the outer controller plus the set point
float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
float throttle_desired = pid_apply_antiwindup(&velocity_pid,
velocity_desired - velocity_z,
0, 1.0f, // positive limits since this is throttle
dt_s);
if (altitudeHoldSettings.AttitudeComp == ALTITUDEHOLDSETTINGS_ATTITUDECOMP_TRUE) {
// Throttle desired is at this point the mount desired in the up direction, we can
// account for the attitude if desired
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
// Project a unit vector pointing up into the body frame and
// get the z component
float fraction = attitudeActual.q1 * attitudeActual.q1 -
attitudeActual.q2 * attitudeActual.q2 -
attitudeActual.q3 * attitudeActual.q3 +
attitudeActual.q4 * attitudeActual.q4;
// Dividing by the fraction remaining in the vertical projection will
// attempt to compensate for tilt. This acts like the thrust is linear
// with the output which isn't really true. If the fraction is starting
// to go negative we are inverted and should shut off throttle
throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f;
}
StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = bound_min_max(throttle_desired, 0.0f, 1.0f);
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
}
}
}
void os_sleep_ms(int ms)
{
tm_wkafter(MS2TICKS(ms));
}
/**
*
* @brief Retrieves an item from the front of a queue.
*
* @param[in] queuep pointer to instance of @p struct pios_queue
* @param[in] itemp pointer to item which will be retrieved
* @param[in] timeout_ms timeout for retrieving item from queue in milliseconds
*
* @returns true on success or false on timeout or failure
*
*/
bool PIOS_Queue_Receive(struct pios_queue *queuep, void *itemp, uint32_t timeout_ms)
{
return xQueueReceive((xQueueHandle)queuep->queue_handle, itemp, MS2TICKS(timeout_ms)) == pdTRUE;
}
/**
* Main task. It does not return.
*/
static void batteryTask(void * parameters)
{
const float dT = SAMPLE_PERIOD_MS / 1000.0f;
settingsUpdatedCb(NULL);
// Main task loop
portTickType lastSysTime;
lastSysTime = xTaskGetTickCount();
while (true) {
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLE_PERIOD_MS));
FlightBatteryStateData flightBatteryData;
FlightBatterySettingsData batterySettings;
float energyRemaining;
if (battery_settings_updated) {
battery_settings_updated = false;
FlightBatterySettingsGet(&batterySettings);
voltageADCPin = batterySettings.VoltagePin;
if (voltageADCPin == FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
voltageADCPin = -1;
currentADCPin = batterySettings.CurrentPin;
if (currentADCPin == FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
currentADCPin = -1;
}
//calculate the battery parameters
if (voltageADCPin >= 0) {
flightBatteryData.Voltage = ((float) PIOS_ADC_GetChannelVolt(voltageADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_VOLTAGE] * 1000.0f +
batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_VOLTAGE]; //in Volts
} else {
flightBatteryData.Voltage = 0; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
if (currentADCPin >= 0) {
flightBatteryData.Current = ((float) PIOS_ADC_GetChannelVolt(currentADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_CURRENT] * 1000.0f +
batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_CURRENT]; //in Amps
if (flightBatteryData.Current > flightBatteryData.PeakCurrent)
flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
} else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
flightBatteryData.Current = -1; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); //in mAh
//Apply a 2 second rise time low-pass filter to average the current
float alpha = 1.0f - dT / (dT + 2.0f);
flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; //in Amps
energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
if (flightBatteryData.AvgCurrent > 0)
flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; //in Sec
else
flightBatteryData.EstimatedFlightTime = 9999;
//generate alarms where needed...
if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) {
//FIXME: There's no guarantee that a floating ADC will give 0. So this
// check might fail, even when there's nothing attached.
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
} else {
// FIXME: should make the timer alarms user configurable
if (flightBatteryData.EstimatedFlightTime < 30)
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.EstimatedFlightTime < 120)
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
// FIXME: should make the battery voltage detection dependent on battery type.
/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
}
FlightBatteryStateSet(&flightBatteryData);
}
}
/**
*
* @brief Appends an item to a queue.
*
* @param[in] queuep pointer to instance of @p struct pios_queue
* @param[in] itemp pointer to item which will be appended to the queue
* @param[in] timeout_ms timeout for appending item to queue in milliseconds
*
* @returns true on success or false on timeout or failure
*
*/
bool PIOS_Queue_Send(struct pios_queue *queuep, const void *itemp, uint32_t timeout_ms)
{
return xQueueSendToBack((xQueueHandle)queuep->queue_handle, itemp, MS2TICKS(timeout_ms)) == pdTRUE;
}
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod));
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
for (uint32_t i = 0; i < VTOL_PID_NUM; i++)
pid_zero(&vtol_pids[i]);
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
#ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0;
if(airspeedSensorType != AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
//Fetch calibrated airspeed from sensors
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
{ //Have to catch the fallthrough, or else this loop will monopolize the processor!
airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE;
airspeedData.SensorValue=12345;
//Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
}
#ifdef GPS_AIRSPEED_PRESENT
float v_air_GPS=-1.0f;
//Check if sufficient time has passed. This will depend on whether we have a pitot tube
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT
float delT = TICKS2MS(lastSysTime - lastLoopTime) / 1000.0f;
lastLoopTime=lastSysTime;
if ( (TICKS2MS(lastSysTime - lastGPSTime) > 1000 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani
gps_airspeedGet(&v_air_GPS);
}
//Use the GPS error to correct the airspeed estimate.
if (v_air_GPS > 0) //We have valid GPS estimate...
{
airspeedData.GPSAirspeed=v_air_GPS;
#ifdef BARO_AIRSPEED_PRESENT
if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present...
//Calculate error and error integral
float airspeedErr=v_air_GPS - airspeed_tas_baro;
airspeedErrInt+=airspeedErr * delT;
//Saturate integral component at 5 m/s
airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
{
//...there's no airspeed sensor, so everything comes from GPS. In this
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
airspeedData.CalibratedAirspeed = tas2cas(airspeedData.TrueAirspeed, -positionActual_Down, &air_STP);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin_dummy)
{
//Find out which sensor we're using.
switch (airspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
break;
}
}
#endif
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}
