Hi
I am developing a module that will subscribe to the attitude setpoint, store between 20 and 100 values and use them to calculate a new attitude setpoint that will get published on a different topic (I haven’t done the publishing part yet) Once the buffer is filled, it goes back to the first value and overwrites it with a new one. I start the application through the nsh console.
My code seems to run fine while on a SITL simulation with jMAVsim. I uploaded it to a pixracer and connected to the console through the USB port in the pixracer. However, when I try to run my application, the connection dies and then I cannot connect to the pixracer even if I turn it on and off. I have to unplug it for a couple of minutes and try again.
I have no idea what could be the problem. I do need my application to run at a high rate, so I don’t know if that could be causing problems. Should I add a sleep command inside the while loop? How short does this sleep period need to be? How big can I make my buffer considering it is an array of attitude setpoint structures?
Here is my code:
static bool thread_should_exit = false; /< daemon exit flag */
static bool thread_running = false; /< daemon status flag */
static int _control_task; /**< Handle of daemon task / thread */
/**
- daemon management function.
*/
__EXPORT int att_command_proc_main(int argc, char *argv[]);
/**
- Mainloop of daemon.
*/
int att_command_proc_thread_main(int argc, char *argv[]);
/**
- Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason) {
warnx("%s\n", reason);
}
warnx("usage: att_command_proc {start|stop|status} [-p <additional params>]\n\n");
}
/**
-
The daemon app only briefly exists to start
-
the background job. The stack size assigned in the
-
Makefile does only apply to this management task.
-
The actual stack size should be set in the call
-
to task_create().
*/
int att_command_proc_main(int argc, char *argv[])
{
if (argc < 2) {
usage(“missing command”);
return 1;
}if (!strcmp(argv[1], “start”)) {
if (thread_running) { warnx("daemon already running\n"); /* this is not an error */ return 0; } thread_should_exit = false; _control_task = px4_task_spawn_cmd("att_command_proc", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, att_command_proc_thread_main, (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0;}
if (!strcmp(argv[1], “stop”)) {
thread_should_exit = true;
return 0;
}if (!strcmp(argv[1], “status”)) {
if (thread_running) {
warnx("\trunning\n");} else { warnx("\tnot started\n"); } return 0;}
usage(“unrecognized command”);
return 1;
}
int att_command_proc_thread_main(int argc, char *argv[])
{
warnx("[daemon] starting\n");
thread_running = true;
/*Variable declarations*/
int _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); //subscribe to attitude setpoint
orb_set_interval(_v_att_sp_sub,20); //update every 20 ms
bool updated;
static uint64_t last_sample_time = 0; //Initialize time in microseconds
float dt=(hrt_absolute_time()-last_sample_time)/1000000.0f;
float samplingInterval = 0.05f;
struct vehicle_attitude_setpoint_s _v_att_sp;
/*Initialize buffer*/
int bufferSize=20;
struct vehicle_attitude_setpoint_s _v_att_sp_proc_buffer[bufferSize];
memset(&_v_att_sp_proc_buffer,0,sizeof(_v_att_sp_proc_buffer));
int procIndex =0;
/*Initialize attitude setpoint to publish*/
struct vehicle_attitude_setpoint_s _v_att_sp_proc;
memset(&_v_att_sp_proc,0,sizeof(_v_att_sp_proc));
/*Initialize processing parameters*/
double A1=0,A2=0,t2=0;
double K,tau;
/*Calculate processing*/
K=1;
tau=0.1;
A1=1/(1+K);
A2=K/(1+K);
t2=tau/2;
int count = 0;
while (!thread_should_exit) {
orb_check(_v_att_sp_sub,&updated);
if(updated){
orb_copy(ORB_ID(vehicle_attitude_setpoint),_v_att_sp_sub,&_v_att_sp);
}
dt=(hrt_absolute_time()-last_sample_time)/1000000.0f;
if (dt>=samplingInterval){
_v_att_sp_proc_buffer[procIndex].roll_body=_v_att_sp_proc_buffer[procIndex].roll_body+_v_att_sp.roll_body*(float)A1;
_v_att_sp_proc_buffer[(procIndex+(int)(t2/(double)samplingInterval))%(bufferSize-1)].roll_body=_v_att_sp.roll_body*(float)A2;
_v_att_sp_proc=_v_att_sp_proc_buffer[procIndex];
memset(&_v_att_sp_proc_buffer[procIndex],0,sizeof(_v_att_sp_proc_buffer[procIndex]));
if(procIndex+1<bufferSize-1)
{
procIndex++;
}
else
{
procIndex=0;
}
last_sample_time = hrt_absolute_time();
//Print processed value to screen
count++;
if (count == 4){
count = 0;
PX4_INFO("Roll: %f\tprocRoll: %f",(double)_v_att_sp.roll_body,(double)_v_att_sp_proc.roll_body);
}
}
}
warnx("[daemon] exiting.\n");
thread_running = false;
return 0;
}