rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1350 split Init function out of Handler, added missing UAVO initalize, cleanup code

Browse Source
This commit is contained in:
Alessio Morale 2014-05-16 18:31:49 +02:00
parent 8dcee11cb8
commit edba1bc71d
3 changed files with 27 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -80,7 +80,12 @@ void pathPlannerHandler(bool newinit);
* @input: NONE:
* @output: NONE
*/
void takeOffLocationHandler(bool newinit);
void takeOffLocationHandler();
/**
* @brief Initialize TakeoffLocationHanlder
*/
void takeOffLocationHandlerInit();
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between

4
flight/modules/ManualControl/manualcontrol.c
View File

@ -135,7 +135,7 @@ int32_t ManualControlStart()
armHandler(true);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandler(true);
takeOffLocationHandlerInit();
#endif
// Start main task
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
@ -171,7 +171,7 @@ static void manualControlTask(void)
// Process Arming
armHandler(false);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandler(false);
takeOffLocationHandler();
#endif
// Process flight mode
FlightStatusData flightStatus;

34
flight/modules/ManualControl/takeofflocationhandler.c
View File

@ -43,27 +43,30 @@ static HandlerStatus_t handlerStatus;
// Private functions
static void SetTakeOffLocation();
void takeOffLocationHandlerInit()
{
TakeOffLocationInitialize();
// check whether there is a preset/valid takeoff location
uint8_t mode;
uint8_t status;
TakeOffLocationModeGet(&mode);
TakeOffLocationStatusGet(&status);
if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
handlerStatus = HANDLER_STATUS_SET;
} else {
handlerStatus = HANDLER_STATUS_UNSET;
status = TAKEOFFLOCATION_STATUS_INVALID;
TakeOffLocationStatusSet(&status);
}
}
/**
* Handles TakeOffPosition location setup
* @param newinit
*/
void takeOffLocationHandler(bool newinit)
void takeOffLocationHandler()
{
if (newinit) {
// check whether there is a preset/valid takeoff location
uint8_t mode;
uint8_t status;
TakeOffLocationModeGet(&mode);
TakeOffLocationStatusGet(&status);
if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
handlerStatus = HANDLER_STATUS_SET;
} else {
handlerStatus = HANDLER_STATUS_UNSET;
}
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
// Location already acquired/preset
@ -121,5 +124,6 @@ void SetTakeOffLocation()
takeOffLocation.North = positionState.North;
takeOffLocation.East = positionState.East;
takeOffLocation.Down = positionState.Down;
TakeOffLocationSet(&takeOffLocation);
handlerStatus = HANDLER_STATUS_PENDING;
}