/* flag == 3 */
static int eventout(scicos_block *block)
{
struct FlagTestDev * comdev = (struct FlagTestDev *) (*block->work);
if (get_scicos_time()==comdev->time){
comdev->step++;
} else {
comdev->time=get_scicos_time();
comdev->step=1;
}
printf("Flag 3: update event output, step=%d, t=%f\n",comdev->step,comdev->time);
return 0;
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void backlash(scicos_block *block,int flag)
{
double* rw = NULL,t = 0.;
if (flag == 4){/* the workspace is used to store previous values */
if ((*block->work= scicos_malloc(sizeof(double)* 4))== NULL ) {
set_block_error(-16);
return;
}
rw=*block->work;
t=get_scicos_time();
rw[0]=t;
rw[1]=t;
rw[2]=block->rpar[0];
rw[3]=block->rpar[0];
}else if (flag == 5){
scicos_free(*block->work);
}else if (flag == 1) {
rw=*block->work;
t=get_scicos_time();
if(t>rw[1]) {
rw[0]=rw[1];
rw[2]=rw[3];
}
rw[1]=t;
if(block->inptr[0][0]>rw[2]+block->rpar[1]/2){
rw[3]=block->inptr[0][0]-block->rpar[1]/2;
} else if (block->inptr[0][0]<rw[2]-block->rpar[1]/2){
rw[3]=block->inptr[0][0]+block->rpar[1]/2;
} else {
rw[3]=rw[2];
}
block->outptr[0][0]=rw[3];
} else if (flag == 9) {
rw=*block->work;
t=get_scicos_time();
if(t>rw[1]){
block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[3];
block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[3];
}else{
block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[2];
block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[2];
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void m_frequ(scicos_block *block, int flag)
{
double *mat = NULL;
double *Dt = NULL;
double *off = NULL;
SCSINT32_COP *icount = NULL;
double t = 0.0;
time_counter_t** work = (time_counter_t**) block->work;
time_counter_t *counter = NULL;
int m = 0;
mat = GetRealOparPtrs(block, 1);
Dt = GetRealOparPtrs(block, 2);
off = GetRealOparPtrs(block, 3);
icount = Getint32OparPtrs(block, 4);
m = GetOparSize(block, 1, 1);
switch (flag)
{
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t) * 2)) == NULL)
{
set_block_error(-16);
return;
}
counter = *work;
*counter = *icount;
(*(counter + 1)) = 0;
break;
}
/* event date computation */
case 3 :
{
counter = *work;
t = get_scicos_time();
*counter += (int)mat[*(counter + 1)]; /*increase counter*/
block->evout[(int)mat[*(counter + 1) + m] - 1] = *off + ((double) * counter / (*Dt)) - t;
(*(counter + 1))++;
*(counter + 1) = *(counter + 1) % m;
break;
}
/* finish */
case 5 :
{
scicos_free(*work); /*free the workspace*/
break;
}
default :
break;
}
}
static void inout(scicos_block *block)
{
double w,pi=3.1415927;
double t = get_scicos_time();
double * rpar = GetRparPtrs(block);
double *y = block->outptr[0];
if (t<rpar[4]) y[0]=0.0;
else {
w=2*pi*rpar[1]*(t-rpar[4])-rpar[2];
y[0]=rpar[0]*sin(w)+rpar[3];
}
}
static int inout(scicos_block *block)
{
struct FlagTestDev * comdev = (struct FlagTestDev *) (*block->work);
int i;
if (get_scicos_time()==comdev->time){
comdev->step++;
} else {
comdev->time=get_scicos_time();
comdev->step=1;
}
printf("Flag 1: update output, step=%d, t=%f\n",comdev->step,comdev->time);
if (GetNin(block)>0){
double *u1 = GetRealInPortPtrs(block,1);
comdev->data=u1[0];
}
if (GetNout(block)>0){
double *y1 = GetRealOutPortPtrs(block,1);
y1[0]=comdev->data;
}
return 0;
}
/* Copyright INRIA
* Scicos block simulator
* event delay with discrete counter
*/
SCICOS_BLOCKS_IMPEXP void evtdly4(scicos_block *block, int flag)
{
double t = 0.;
time_counter_t** work = (time_counter_t**) block->work;
time_counter_t* i = NULL;
switch (flag)
{
/* init */
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t))) == NULL)
{
set_block_error(-16);
return;
}
i = *work;
(*i) = 0;
break;
}
/* event date computation */
case 3 :
{
double dt;
i = *work;
t = get_scicos_time();
(*i)++; /*increase counter*/
dt = block->rpar[1] + (*i) * block->rpar[0] - t;
/* on event enabled, use the default delay if not scheduled */
if (block->rpar[1] >= 0 && dt < 0)
{
dt = block->rpar[0];
}
block->evout[0] = dt;
break;
}
/* finish */
case 5 :
{
scicos_free(*work); /*free the workspace*/
break;
}
default :
break;
}
}
static void inout(scicos_block *block)
{
double *u;
int ntraces=block->nin;
struct {
float t;
float u[ntraces];
} data;
int i;
data.t=(float) get_scicos_time();
for (i = 0; i < ntraces; i++) {
u=block->inptr[i];
data.u[i] = (float) u[0];
}
rtf_put(block->ipar[0],&data, sizeof(data));
}
static void inout(scicos_block *block)
{
double *u;
MBX *mbx = *(block->work);
int ntraces=GetNin(block);
struct {
double t;
double u[ntraces];
} data;
int i;
data.t=get_scicos_time();
for (i = 0; i < ntraces; i++) {
u = block->inptr[i];
data.u[i] = u[0];
}
RT_mbx_send_if(0, 0, mbx, &data, sizeof(data));
}
/** \fn void cscope(scicos_block * block,int flag)
\brief the computational function
\param block A pointer to a scicos_block
\param flag An int which indicates the state of the block (init, update, ending)
*/
SCICOS_BLOCKS_IMPEXP void cevscpe(scicos_block * block, scicos_flag flag)
{
int iFigureUID;
double t;
int i;
int mask;
int nclk = block->nipar - 6;
sco_data *sco;
BOOL result;
switch (flag)
{
case Initialization:
sco = getScoData(block);
if (sco == NULL)
{
set_block_error(-5);
}
iFigureUID = getFigure(block);
if (iFigureUID == 0)
{
// allocation error
set_block_error(-5);
break;
}
setSegsBuffers(block, DEFAULT_MAX_NUMBER_OF_POINTS);
break;
case StateUpdate:
iFigureUID = getFigure(block);
if (iFigureUID == 0)
{
// allocation error
set_block_error(-5);
break;
}
t = get_scicos_time();
// select only the masked indexes
for (i = 0; i < nclk; i++)
{
mask = 1 << i;
if ((block->nevprt & mask) == mask)
{
appendData(block, i, t);
result = pushData(block, i);
if (result == FALSE)
{
Coserror("%s: unable to push some data.", "cevscpe");
break;
}
}
}
break;
case Ending:
freeScoData(block);
break;
default:
break;
}
}
void sci_mavlinkHilState(scicos_block *block, scicos::enumScicosFlags flag)
{
// data
double * u=GetRealInPortPtrs(block,1);
double * y=GetRealOutPortPtrs(block,1);
int * ipar=block->ipar;
static char * device;
static int baudRate;
static char ** stringArray;
static int * intArray;
static int count = 0;
static uint16_t packet_drops = 0;
//handle flags
if (flag==scicos::initialize || flag==scicos::reinitialize)
{
if (mavlink_comm_0_port == NULL)
{
getIpars(1,1,ipar,&stringArray,&intArray);
device = stringArray[0];
baudRate = intArray[0];
try
{
mavlink_comm_0_port = new BufferedAsyncSerial(device,baudRate);
}
catch(const boost::system::system_error & e)
{
Coserror((char *)e.what());
}
}
}
else if (flag==scicos::terminate)
{
if (mavlink_comm_0_port)
{
delete mavlink_comm_0_port;
mavlink_comm_0_port = NULL;
}
}
else if (flag==scicos::updateState)
{
}
else if (flag==scicos::computeDeriv)
{
}
else if (flag==scicos::computeOutput)
{
// channel
mavlink_channel_t chan = MAVLINK_COMM_0;
// loop rates
// TODO: clean this up to use scicos events w/ timers
static int attitudeRate = 50;
static int positionRate = 10;
static int airspeedRate = 1;
// initial times
double scicosTime = get_scicos_time();
static double attitudeTimeStamp = scicosTime;
static double positionTimeStamp = scicosTime;
static double airspeedTimeStamp = scicosTime;
// send airspeed message
if (scicosTime - airspeedTimeStamp > 1.0/airspeedRate)
{
airspeedTimeStamp = scicosTime;
// airspeed (true velocity m/s)
float Vt = u[0];
//double rawPress = 1;
//double airspeed = 1;
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - airspeedTimeStamp < 0)
airspeedTimeStamp = scicosTime;
// send attitude message
if (scicosTime - attitudeTimeStamp > 1.0/attitudeRate)
{
attitudeTimeStamp = scicosTime;
// attitude states (rad)
float roll = u[1];
float pitch = u[2];
float yaw = u[3];
// body rates
float rollRate = u[4];
float pitchRate = u[5];
float yawRate = u[6];
mavlink_msg_attitude_send(chan,attitudeTimeStamp,roll,pitch,yaw,
rollRate,pitchRate,yawRate);
}
else if (scicosTime - attitudeTimeStamp < 0)
attitudeTimeStamp = scicosTime;
// send gps mesage
if (scicosTime - positionTimeStamp > 1.0/positionRate)
{
positionTimeStamp = scicosTime;
// gps
double cog = u[7];
double sog = u[8];
double lat = u[9]*rad2deg;
double lon = u[10]*rad2deg;
double alt = u[11];
//double rawPress = 1;
//double airspeed = 1;
mavlink_msg_gps_raw_send(chan,positionTimeStamp,1,lat,lon,alt,2,10,sog,cog);
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - positionTimeStamp < 0)
positionTimeStamp = scicosTime;
// receive messages
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(MAVLINK_COMM_0))
{
uint8_t c = comm_receive_ch(MAVLINK_COMM_0);
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving messages" << std::endl;
mavlink_rc_channels_scaled_t rc_channels;
mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels);
y[0] = rc_channels.chan1_scaled/10000.0f;
y[1] = rc_channels.chan2_scaled/10000.0f;
y[2] = rc_channels.chan3_scaled/10000.0f;
y[3] = rc_channels.chan4_scaled/10000.0f;
y[4] = rc_channels.chan5_scaled/10000.0f;
y[5] = rc_channels.chan6_scaled/10000.0f;
y[6] = rc_channels.chan7_scaled/10000.0f;
y[7] = rc_channels.chan8_scaled/10000.0f;
break;
}
}
// update packet drop counter
packet_drops += status.packet_rx_drop_count;
}
}
}
}
/** \fn void cmscope(scicos_block * block,int flag)
\brief the computational function
\param block A pointer to a scicos_block
\param flag An int which indicates the state of the block (init, update, ending)
*/
SCICOS_BLOCKS_IMPEXP void cmscope(scicos_block * block, scicos_flag flag)
{
char const* pFigureUID;
double t;
double *u;
sco_data *sco;
int i, j;
BOOL result;
switch (flag)
{
case Initialization:
sco = getScoData(block);
if (sco == NULL)
{
set_block_error(-5);
break;
}
pFigureUID = getFigure(block);
if (pFigureUID == NULL)
{
// allocation error
set_block_error(-5);
break;
}
break;
case StateUpdate:
pFigureUID = getFigure(block);
if (pFigureUID == NULL)
{
// allocation error
set_block_error(-5);
break;
}
t = get_scicos_time();
for (i = 0; i < block->nin; i++)
{
u = (double *)block->inptr[i];
appendData(block, i, t, u);
for (j = 0; j < block->insz[i]; j++)
{
result = pushData(block, i, j);
if (result == FALSE)
{
Coserror("%s: unable to push some data.", "cmscope");
break;
}
}
}
break;
case Ending:
freeScoData(block);
break;
default:
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void time_delay(scicos_block *block, int flag)
{
/* rpar[0]=delay, rpar[1]=init value, ipar[0]=buffer length */
double** work = (double**) block->work;
double *pw = NULL, del = 0., t = 0., td = 0., eps = 0.;
int* iw = NULL;
int i = 0, j = 0, k = 0;
if (flag == 4)
{
/* the workspace is used to store previous values */
if ((*work = (double*) scicos_malloc(sizeof(int) + sizeof(double) *
block->ipar[0] * (1 + block->insz[0]))) == NULL )
{
set_block_error(-16);
return;
}
eps = 1.0e-9; /* shift times to left to avoid replacing 0 */
pw = *work;
pw[0] = -block->rpar[0] * (block->ipar[0] - 1) - eps;
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[block->ipar[0]*j] = block->rpar[1];
}
for (i = 1; i < block->ipar[0]; i++)
{
pw[i] = pw[i - 1] + block->rpar[0] - eps;
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[i + block->ipar[0]*j] = block->rpar[1];
}
}
iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0]));
*iw = 0;
for (k = 0; k < block->insz[0]; k++)
{
block->outptr[0][k] = block->rpar[1];
}
}
else if (flag == 5)
{
scicos_free(*work);
}
else if (flag == 0 || flag == 2)
{
if (flag == 2)
{
do_cold_restart();
}
pw = *work;
iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
td = t - block->rpar[0];
if (td < pw[*iw])
{
scicos_print(_("delayed time=%f but last stored time=%f \n"), td, pw[*iw]);
scicos_print(_("Consider increasing the length of buffer in delay block \n"));
}
if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]])
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[*iw] = t;
/*scicos_print(_("**time is %f. I put %f, in %d \n"), t,block->inptr[0][0],*iw);*/
*iw = (*iw + 1) % block->ipar[0];
}
else
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t;
/*scicos_print("**time is %f. I put %f, in %d \n", t,block->inptr[0][0],*iw);*/
}
}
else if (flag == 1)
{
pw = *work;
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
td = t - block->rpar[0];
i = 0;
j = block->ipar[0] - 1;
while (j - i > 1)
{
k = (i + j) / 2;
if (td < pw[(k + *iw) % block->ipar[0]])
{
j = k;
}
else if (td > pw[(k + *iw) % block->ipar[0]])
{
i = k;
}
else
{
i = k;
j = k;
break;
}
}
i = (i + *iw) % block->ipar[0];
j = (j + *iw) % block->ipar[0];
del = pw[j] - pw[i];
/* scicos_print(_("time is %f. interpolating %d and %d, i.e. %f, %f\n"), t,i,j,pw[i],pw[j]);
scicos_print(_("values are %f %f.\n"),pw[i+block->ipar[0]],pw[j+block->ipar[0]]);*/
if (del != 0.0)
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] +
(td - pw[i]) * pw[j + block->ipar[0] * k]) / del;
}
}
else
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = pw[i + block->ipar[0] * k];
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void variable_delay(scicos_block *block, int flag)
{
/* rpar[0]=max delay, rpar[1]=init value, ipar[0]=buffer length */
double** work = (double**) block->work;
double* pw = NULL, del = 0., t = 0., td = 0.;
int* iw = NULL;
int i = 0, j = 0, k = 0;
if (flag == 4) /* the workspace is used to store previous values */
{
if ((*work = (double*)
scicos_malloc(sizeof(int) + sizeof(double) *
block->ipar[0] * (1 + block->insz[0]))) == NULL )
{
set_block_error(-16);
return;
}
pw = *work;
pw[0] = -block->rpar[0] * block->ipar[0];
for (i = 1; i < block->ipar[0]; i++)
{
pw[i] = pw[i - 1] + block->rpar[0];
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[i + block->ipar[0]*j] = block->rpar[1];
}
}
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
*iw = 0;
}
else if (flag == 5)
{
scicos_free(*work);
}
else if (flag == 1)
{
if (get_phase_simulation() == 1)
{
do_cold_restart();
}
pw = *work;
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
del = Min(Max(0, block->inptr[1][0]), block->rpar[0]);
td = t - del;
if (td < pw[*iw])
{
scicos_print(_("delayed time=%f but last stored time=%f\n"), td, pw[*iw]);
scicos_print(_("Consider increasing the length of buffer in variable delay block\n"));
}
if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]])
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[*iw] = t;
*iw = (*iw + 1) % block->ipar[0];
}
else
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t;
}
i = 0;
j = block->ipar[0] - 1;
while (j - i > 1)
{
k = (i + j) / 2;
if (td < pw[(k + *iw) % block->ipar[0]])
{
j = k;
}
else if (td > pw[(k + *iw) % block->ipar[0]])
{
i = k;
}
else
{
i = k;
j = k;
break;
}
}
i = (i + *iw) % block->ipar[0];
j = (j + *iw) % block->ipar[0];
del = pw[j] - pw[i];
if (del != 0.0)
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] +
(td - pw[i]) * pw[j + block->ipar[0] * k]) / del;
}
}
else
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = pw[i + block->ipar[0] * k];
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void ratelimiter(scicos_block *block,int flag)
{
/* rpar[0]=rising rate limit, rpar[1]=falling rate limit */
double* pw = NULL;
double rate = 0. , t = 0.;
if (flag == 4)
{
/* the workspace is used to store previous values */
if ((*block->work = scicos_malloc(sizeof(double)*4))== NULL )
{
set_block_error(-16);
return;
}
pw=*block->work;
pw[0]=0.0;
pw[1]=0.0;
pw[2]=0.0;
pw[3]=0.0;
}
else if (flag == 5)
{
scicos_free(*block->work);
}
else if (flag==1)
{
if (get_phase_simulation()==1) do_cold_restart();
pw=*block->work;
t=get_scicos_time();
if(t>pw[2])
{
pw[0]=pw[2];
pw[1]=pw[3];
rate=(block->inptr[0][0]-pw[1])/(t-pw[0]);
}
else if(t<=pw[2])
{
if(t>pw[0])
{
rate=(block->inptr[0][0]-pw[1])/(t-pw[0]);
}
else
{
rate=0.0;
}
}
if(rate>block->rpar[0])
{
block->outptr[0][0]=(t-pw[0])*block->rpar[0]+pw[1];
}
else if(rate<block->rpar[1])
{
block->outptr[0][0]=(t-pw[0])*block->rpar[1]+pw[1];
}
else
{
block->outptr[0][0]=block->inptr[0][0];
}
pw[2]=t;
pw[3]=block->outptr[0][0];
}
}
void sci_mavlinkHilTracker(scicos_block *block, scicos::enumScicosFlags flag)
{
// data
double * u=GetRealInPortPtrs(block,1);
double * y=GetRealOutPortPtrs(block,1);
int * ipar=block->ipar;
static char * device;
static int baudRate;
static char ** stringArray;
static int * intArray;
static int count = 0;
static uint16_t packet_drops = 0;
//handle flags
if (flag==scicos::initialize || flag==scicos::reinitialize)
{
if (mavlink_comm_2_port == NULL)
{
getIpars(1,1,ipar,&stringArray,&intArray);
device = stringArray[0];
baudRate = intArray[0];
try
{
mavlink_comm_2_port = new BufferedAsyncSerial(device,baudRate);
}
catch(const boost::system::system_error & e)
{
Coserror((char *)e.what());
}
}
}
else if (flag==scicos::terminate)
{
if (mavlink_comm_2_port)
{
delete mavlink_comm_2_port;
mavlink_comm_2_port = NULL;
}
}
else if (flag==scicos::updateState)
{
}
else if (flag==scicos::computeDeriv)
{
}
else if (flag==scicos::computeOutput)
{
// channel
mavlink_channel_t chan = MAVLINK_COMM_2;
// loop rates
// TODO: clean this up to use scicos events w/ timers
static int positionRate = 10;
// initial times
double scicosTime = get_scicos_time();
static double positionTimeStamp = scicosTime;
// send global position
if (scicosTime - positionTimeStamp > 1.0/positionRate)
{
positionTimeStamp = scicosTime;
// gps
double lat = u[0]*rad2deg;
double lon = u[1]*rad2deg;
double alt = u[2];
double vN = u[3];
double vE = u[4];
double vD = u[5];
mavlink_msg_global_position_send(chan,positionTimeStamp,lat,lon,alt,vN,vE,vD);
//std::cout << "sending global position" << std::endl;
}
else if (scicosTime - positionTimeStamp < 0)
positionTimeStamp = scicosTime;
// receive messages
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(MAVLINK_COMM_2))
{
//std::cout << "serial available" << std::endl;
uint8_t c = comm_receive_ch(MAVLINK_COMM_2);
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_2,c,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving messages" << std::endl;
mavlink_rc_channels_scaled_t rc_channels;
mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels);
y[0] = rc_channels.chan1_scaled/10000.0f;
y[1] = rc_channels.chan2_scaled/10000.0f;
y[2] = rc_channels.chan3_scaled/10000.0f;
y[3] = rc_channels.chan4_scaled/10000.0f;
y[4] = rc_channels.chan5_scaled/10000.0f;
y[5] = rc_channels.chan6_scaled/10000.0f;
y[6] = rc_channels.chan7_scaled/10000.0f;
y[7] = rc_channels.chan8_scaled/10000.0f;
break;
}
}
// update packet drop counter
packet_drops += status.packet_rx_drop_count;
}
}
}
}