mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
fc1e3f574c
Also implement some ordering (quite ugly still) in the module init and task creation order so we can decide which module to start/init first and which module to start/init last. This will be replaced/adapter with the uavobject list later (once it's implemented). reserving some space for module init and task create parameters to customize module/task creation (this will be usefull once we get the list and customization from customer). Changes have been made for OP and CC. Tested comped with CC,OP, sim_posix. Only ran on bench with CC for couple of minutes (code increase expected but no dropping of stack which is good). This gives task creation at the time wherethe all heap is available.
139 lines
4.2 KiB
C
139 lines
4.2 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup AHRSCommsModule AHRSComms Module
|
|
* @brief Handles communication with AHRS and updating position
|
|
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
|
|
* @{
|
|
*
|
|
* @file ahrs_comms.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
******************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
/**
|
|
* Input objects: As defined in PiOS/inc/pios_ahrs_comms.h
|
|
* Output objects: As defined in PiOS/inc/pios_ahrs_comms.h
|
|
*
|
|
* This module will periodically update the values of latest attitude solution
|
|
* and other objects that are transferred to and from the AHRS
|
|
* The module settings can configure how often AHRS is polled for a new solution.
|
|
*
|
|
* The module executes in its own thread.
|
|
*
|
|
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
|
* the object definition XML file.
|
|
*
|
|
* Modules have no API, all communication to other modules is done through UAVObjects.
|
|
* However modules may use the API exposed by shared libraries.
|
|
* See the OpenPilot wiki for more details.
|
|
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
|
*
|
|
*/
|
|
|
|
#include "ahrs_comms.h"
|
|
#include "ahrs_spi_comm.h"
|
|
|
|
// Private constants
|
|
#define STACK_SIZE configMINIMAL_STACK_SIZE-128
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static xTaskHandle taskHandle;
|
|
|
|
// Private functions
|
|
static void ahrscommsTask(void *parameters);
|
|
|
|
/**
|
|
* Initialise the module, called on startup
|
|
* \returns 0 on success or -1 if initialisation failed
|
|
*/
|
|
int32_t AHRSCommsStart(void)
|
|
{
|
|
// Start main task
|
|
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
|
TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle);
|
|
PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Initialise the module, called on startup
|
|
* \returns 0 on success or -1 if initialisation failed
|
|
*/
|
|
int32_t AHRSCommsInitialize(void)
|
|
{
|
|
|
|
return 0;
|
|
}
|
|
module_initcall(AHRSCommsInitialize, 0, AHRSCommsStart, 0, MODULE_EXEC_NOORDER_FLAG);
|
|
|
|
/**
|
|
* Module thread, should not return.
|
|
*/
|
|
static void ahrscommsTask(void *parameters)
|
|
{
|
|
portTickType lastSysTime;
|
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
|
|
|
|
// Main task loop
|
|
while (1) {
|
|
PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
|
|
|
|
AHRSSettingsData settings;
|
|
AHRSSettingsGet(&settings);
|
|
|
|
AhrsSendObjects();
|
|
AhrsCommStatus stat = AhrsGetStatus();
|
|
if (stat.linkOk) {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
|
|
} else {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
|
|
}
|
|
AhrsStatusData sData;
|
|
AhrsStatusGet(&sData);
|
|
|
|
sData.LinkRunning = stat.linkOk;
|
|
sData.AhrsKickstarts = stat.remote.kickStarts;
|
|
sData.AhrsCrcErrors = stat.remote.crcErrors;
|
|
sData.AhrsRetries = stat.remote.retries;
|
|
sData.AhrsInvalidPackets = stat.remote.invalidPacket;
|
|
sData.OpCrcErrors = stat.local.crcErrors;
|
|
sData.OpRetries = stat.local.retries;
|
|
sData.OpInvalidPackets = stat.local.invalidPacket;
|
|
|
|
AhrsStatusSet(&sData);
|
|
/* Wait for the next update interval */
|
|
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
|
|
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|