ICAROUS
All Classes Namespaces Files Functions Variables Enumerations Enumerator Macros Modules Pages
rotorsim.h
Go to the documentation of this file.
1
10#ifndef ICAROUS_CFS_ROTORSIM_H
11#define ICAROUS_CFS_ROTORSIM_H
12
13#include "cfe.h"
14#include "cfe_error.h"
15#include "cfe_evs.h"
16#include "cfe_sb.h"
17#include "cfe_es.h"
18#include "cfe_platform_cfg.h"
19
20#include <string.h>
21#include "rotorsim_table.h"
22
23#include "Icarous_msgids.h"
24#include "Icarous_msg.h"
25#include "sch_msgids.h"
26
27#define ROTORSIM_STARTUP_INF_EID 0
28#define ROTORSIM_COMMAND_ERR_EID 1
29
30#define ROTORSIM_PIPE_DEPTH 100
31#define ROTORSIM_PIPE_NAME "ROTORSIM_PIPE"
32#define ROTORSIM_MAJOR_VERSION 1
33#define ROTORSIM_MINOR_VERSION 0
34
48typedef struct{
49 CFE_SB_PipeId_t Rotorsim_Pipe;
50 CFE_SB_MsgPtr_t Rotorsim_MsgPtr;
52 double externalCmdV[3];
53 double externalCmdP[3];
54 double position[3];
55 double velocity[3];
56 double attitude[3];
57 double heading;
58 bool passive;
64 int nextWP;
66 uint32_t timerId;
67 double time;
68 double offsetdist;
69 uint32_t velcount;
71
76
80void Rotorsim_AppInit(void);
81
86
90void Rotorsim_AppCleanUp(void);
91
95void Rotorsim_ProcessPacket(void);
96
102
108int32_t RotorsimTableValidationFunc(void *TblPtr);
109
110void Rotorsim_InitModules(RotorsimTable_t* params);
111
112void Rotorsim_GetInputs(void);
113
114void Rotorsim_GetOutputs(void);
115
116void timer_callback(uint32_t timerId);
117
118bool ComputeOffSetPositionOnPlan(double position[],int currentLeg,double output[]);
119
120void GetCorrectIntersectionPoint(double _wpA[],double _wpB[],double heading,double r,double output[]);
121
122double distance(double x1,double y1,double x2,double y2);
123
124#endif //ICAROUS_CFS_ROTORSIM_H
Icarous common message definitions.
Message ID definition of common icarous messages.
EXTERN position_t position
position message
Definition: arducopter.h:225
EXTERN attitude_t attitude
attitude message
Definition: arducopter.h:227
rotorsimAppData_t rotorsimAppData
global variable to hold app data
Definition: rotorsim.h:101
void Rotorsim_AppInitData(RotorsimTable_t *TblPtr)
Definition: rotorsim.c:95
void Rotorsim_ProcessPacket(void)
Definition: rotorsim.c:117
int32_t RotorsimTableValidationFunc(void *TblPtr)
Definition: rotorsim.c:171
void Rotorsim_AppInit(void)
Definition: rotorsim.c:43
void Rotorsim_AppMain(void)
void Rotorsim_AppCleanUp(void)
Definition: rotorsim.c:113
Definition of rotorcraft simulation table.
Rotorsim table data.
Definition: rotorsim_table.h:21
message encoding flight plan information
Struct to hold rotor sim app data.
Definition: rotorsim.h:48
CFE_SB_MsgPtr_t Rotorsim_MsgPtr
Pointer to SB message.
Definition: rotorsim.h:50
double offsetdist
Moving reference point on controller.
Definition: rotorsim.h:68
bool startMission
Start mission.
Definition: rotorsim.h:59
double time
time
Definition: rotorsim.h:67
uint32_t velcount
Velocity counter.
Definition: rotorsim.h:69
bool passive
Passive mode.
Definition: rotorsim.h:58
bool flightplanSent
flight plan send status
Definition: rotorsim.h:62
uint32_t timerId
Timer id.
Definition: rotorsim.h:66
bool velocityControl
velocity control mode
Definition: rotorsim.h:61
flightplan_t flightPlan
Input flight plan.
Definition: rotorsim.h:65
bool flightplanReceived
Received flightplan status.
Definition: rotorsim.h:63
RotorsimTable_t Rotorsim_Tbl
Rotor sim table;.
Definition: rotorsim.h:51
int nextWP
Next waypoint index.
Definition: rotorsim.h:64
bool positionControl
position control mode
Definition: rotorsim.h:60
CFE_SB_PipeId_t Rotorsim_Pipe
Pipe variable.
Definition: rotorsim.h:49