mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
102 lines
2.4 KiB
C
102 lines
2.4 KiB
C
|
|
#include "pios.h"
|
|
#include "pios_sim_priv.h"
|
|
#include "sim_model.h"
|
|
|
|
struct pios_sim_state pios_sim_state = {
|
|
.accels = {0, 0, 0},
|
|
.gyros = {0, 0, 0},
|
|
.mag = {0, 0, 0},
|
|
.baro = {0},
|
|
.q = {1, 0, 0, 0},
|
|
.velocity = {0, 0, 0},
|
|
.position = {0, 0, 0},
|
|
.actuator = {0, 0, 0, 0, 0, 0, 0, 0}
|
|
};
|
|
|
|
/**
|
|
* Initialize the model in the external library
|
|
* @returns 0 for success, -1 if fails to initialize external library
|
|
*/
|
|
int PIOS_SIM_Init()
|
|
{
|
|
if (sim_model_init() != 0)
|
|
return -1;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Step the model simulation in the external library
|
|
* @returns 0 for success, -1 for failure to step external library
|
|
*/
|
|
int PIOS_SIM_Step(float dT)
|
|
{
|
|
if (sim_model_step(dT, &pios_sim_state) != 0)
|
|
return -1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Set the actuator inputs to the model
|
|
* @param[in] actuator pointer to an array of actuators to set
|
|
* @param[in] nchannels number of channels that are valid coming in
|
|
*/
|
|
void PIOS_SIM_SetActuator(float * actuator, int nchannels)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++)
|
|
pios_sim_state.actuator[i] = actuator[i];
|
|
}
|
|
|
|
/**
|
|
* Get the accelerometer data from the simulation model
|
|
* @param[out] pointer to store the accelerometer data in
|
|
*/
|
|
void PIOS_SIM_GetAccels(float * accels)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++)
|
|
accels[i] = pios_sim_state.accels[i];
|
|
}
|
|
|
|
/**
|
|
* Get the gyro data from the simulation model
|
|
* @param[out] pointer to store the gyro data in
|
|
*/
|
|
void PIOS_SIM_GetGyros(float * gyros)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++)
|
|
gyros[i] = pios_sim_state.gyros[i];
|
|
}
|
|
|
|
/**
|
|
* Get the current attitude from the simulation model
|
|
* @param[out] quat pointer to store the quaternion attitude in
|
|
*/
|
|
void PIOS_SIM_GetAttitude(float * q)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++)
|
|
q[i] = pios_sim_state.q[i];
|
|
}
|
|
|
|
/**
|
|
* Get the current positiom from the simulation model
|
|
* @param[out] position pointer to store the current position in (cm in NED
|
|
* frame)
|
|
*/
|
|
void PIOS_SIM_GetVelocity(float * velocity)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++)
|
|
velocity[i] = pios_sim_state.velocity[i];
|
|
}
|
|
|
|
/**
|
|
* Get the current positiom from the simulation model
|
|
* @param[out] position pointer to store the current position in (cm in NED
|
|
* frame)
|
|
*/
|
|
void PIOS_SIM_GetPosition(float * position)
|
|
{
|
|
for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++)
|
|
position[i] = pios_sim_state.position[i];
|
|
}
|