Hi,
I used the Hello Sky example successfully to write my first app. However, I wanted to application to run indefinitely (i.e. not for given number of loops). I tried and tested these approaches in SITL:
- Simply comment out
for loop, but then the application ran and closed immediately - Replace
for loopwithwhile(true), but then the application took over and never returned to the nuttx prompt
Neither technique worked. So I tried to to replicate the fixed wing control example by putting the infrastructure to use px4_task_spawn_cmd(). However, when I run the application (<name> start) in SITL, the application causes immediate termination of the SITL. JMAVSim closes and I’m left in the Ubuntu terminal with Restoring Terminal printed to the screen.
Any ideas on what could cause this? Below is the current state of my application. I also tried commenting out most of ahrs_accel_thread_main, but again the same issue (closing/crashing of SITL) occurred.
CMakeLists.txt
px4_add_module(
MODULE examples__casestudy_ahrs_app
MAIN ex_ahrs_accel
STACK_MAIN 2000
STACK_MAX 1300
SRCS
ahrs_accel_app.c
utils/umn_matrix.c
utils/nav_functions.c
DEPENDS
platforms__common
)
ahrs_accel_app.c
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>
#include <float.h>
// Custum Libraries
#include "utils/umn_matrix.h"
#include "utils/nav_functions.h"
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/casestudy_ahrs.h>
/**
* Daemon management function.
*
* This function allows to start / stop the background task (daemon).
* The purpose of it is to be able to start the controller on the
* command line, query its status and stop it, without giving up
* the command line to one particular process or the need for bg/fg
* ^Z support by the shell.
*/
__EXPORT int ex_ahrs_accel_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
*/
int ahrs_accel_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
// static struct params p;
// static struct param_handles ph;
/* Main Thread */
int ahrs_accel_thread_main(int argc, char *argv[])
{
PX4_INFO("Starting Accel-Based AHRS App (infinite loop).");
// Get topic handle to sensor_combined
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub_fd, 1000);
/* Advertise attitude topic. */
struct casestudy_ahrs_s att;
memset(&att, 0, sizeof(att));
orb_advert_t att_pub = orb_advertise(ORB_ID(casestudy_ahrs), &att);
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
*/
};
int error_counter = 0;
while(!thread_should_exit){
/* wait for sensor update of file descriptor for 1000 ms (1 second) */
int poll_ret = px4_poll(fds, 1, 1000);
/* Handle the poll results. */
if (poll_ret == 0){
/* This means none of our providers are giving us data. */
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
PX4_ERR("[ahrs_accel_app] ERROR return value from poll(): %d"
, poll_ret);
}
error_counter++;
} else {
/* Handle file descriptors. */
if (fds[0].revents & POLLIN){
/* obtain data for the first file descriptor */
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
float fx = raw.accelerometer_m_s2[0]; // units: m/s^2
float fy = raw.accelerometer_m_s2[1];
float fz = raw.accelerometer_m_s2[2];
// PX4_WARN("[ahrs_accel_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
// (double)raw.accelerometer_m_s2[0],
// (double)raw.accelerometer_m_s2[1],
// (double)raw.accelerometer_m_s2[2]);
/* Save attitude using fused solution */
att.sim_att_roll = 0;
att.sim_att_pitch = 0;
att.sim_att_yaw = 0;
/* Save attitude using gyro-only */
att.gyro_att_roll = 0;
att.gyro_att_pitch = 0;
att.gyro_att_yaw = 0;
/* Compute attitude using accelerometers */
att.vect_att_roll = atan2(-fy,-fz);
att.vect_att_pitch = atan2( fx, sqrt(fy*fy + fz*fz));
att.vect_att_yaw = 0;
// PX4_WARN("[ahrs_accel_app] Accel-Based Roll, Pitch:\t%8.4f\t%8.4f\n",
// (double)att.vect_att_roll,
// (double)att.vect_att_pitch);
/* Set att and publish this information for other apps */
orb_publish(ORB_ID(casestudy_ahrs), att_pub, &att);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
} // end while loop
printf("[ex_ahrs_accel] exiting.\n");
thread_running = false;
fflush(stdout);
return 0;
} // end ahrs_accel_app_main
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: ex_ahrs_accel {start|stop|status}\n\n");
exit(1);
}
/**
* 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 px4_task_spawn_cmd().
*/
int ex_ahrs_accel_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ex_ahrs_accel already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("ex_ahrs_accel",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
ahrs_accel_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\ex_ahrs_accel is running\n");
} else {
printf("\ex_ahrs_accel not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}