mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-01 18:29:16 +01:00
548a32f18b
The Attitude module will soon be handling updates for all UAVObjects that require data from the AHRS. To reflect this expansion of scope, it has been renamed to AHRSComms. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1009 ebee16cc-31ac-478f-84a7-5cbb03baadba
121 lines
3.8 KiB
C
121 lines
3.8 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @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 object: AttitudeSettings
|
|
* Output object: AttitudeActual
|
|
*
|
|
* This module will periodically update the value of latest attitude solution
|
|
* that is available 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 "attitudeactual.h" // object that will be updated by the module
|
|
#include "attitudesettings.h" // object holding module settings
|
|
|
|
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
|
|
|
|
// Private constants
|
|
#define STACK_SIZE 200
|
|
#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 AHRSCommsInitialize(void)
|
|
{
|
|
// Start main task
|
|
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Module thread, should not return.
|
|
*/
|
|
static void ahrscommsTask(void* parameters)
|
|
{
|
|
AttitudeSettingsData settings;
|
|
AttitudeActualData data;
|
|
|
|
// Main task loop
|
|
while (1)
|
|
{
|
|
// Update settings with latest value
|
|
AttitudeSettingsGet(&settings);
|
|
|
|
// Get the current object data
|
|
AttitudeActualGet(&data);
|
|
|
|
// Query the latest attitude solution from the AHRS
|
|
PIOS_OPAHRS_ReadAttitude();
|
|
|
|
// Update the data
|
|
data.q1 += 0.111;
|
|
data.q2 += 1.1;
|
|
data.q3 += 7.0;
|
|
data.q4 -= 2.321;
|
|
|
|
data.Roll += 0.01;
|
|
data.Pitch -= 0.03;
|
|
data.Yaw += 0.05;
|
|
|
|
// 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.
|
|
AttitudeActualSet(&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 divide by portTICK_RATE_MS.
|
|
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
|
|
}
|
|
}
|