int test_dataman(int argc, char *argv[])
{
int i, num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1) {
num_tasks = atoi(argv[1]);
}
sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
warnx("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
sprintf(a, "%d", i);
const char *av[2];
av[0] = a;
av[1] = 0;
px4_sem_init(sems + i, 1, 0);
/* sems use case is a signal */
px4_sem_setprotocol(&sems, SEM_PRIO_NONE);
/* start the task */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, (void *)av)) <= 0) {
warn("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}
}
return 0;
}
int
dataman_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -1;
}
if (!strcmp(argv[1], "start")) {
if (g_fd >= 0) {
warnx("dataman already running");
return -1;
}
if (argc == 4 && strcmp(argv[2],"-f") == 0) {
k_data_manager_device_path = strdup(argv[3]);
warnx("dataman file set to: %s\n", k_data_manager_device_path);
}
else {
k_data_manager_device_path = strdup(default_device_path);
}
start();
if (g_fd < 0) {
warnx("dataman start failed");
free(k_data_manager_device_path);
k_data_manager_device_path = NULL;
return -1;
}
return 0;
}
/* Worker thread should be running for all other commands */
if (g_fd < 0) {
warnx("dataman worker thread not running");
usage();
return -1;
}
if (!strcmp(argv[1], "stop")) {
stop();
free(k_data_manager_device_path);
k_data_manager_device_path = NULL;
}
else if (!strcmp(argv[1], "status"))
status();
else if (!strcmp(argv[1], "poweronrestart"))
dm_restart(DM_INIT_REASON_POWER_ON);
else if (!strcmp(argv[1], "inflightrestart"))
dm_restart(DM_INIT_REASON_IN_FLIGHT);
else {
usage();
return -1;
}
return 1;
}
int
dataman_main(int argc, char *argv[])
{
if (argc < 2)
usage();
if (!strcmp(argv[1], "start")) {
if (g_fd >= 0)
errx(1, "already running");
start();
if (g_fd < 0)
errx(1, "start failed");
exit(0);
}
/* Worker thread should be running for all other commands */
if (g_fd < 0)
errx(1, "not running");
if (!strcmp(argv[1], "stop"))
stop();
else if (!strcmp(argv[1], "status"))
status();
else if (!strcmp(argv[1], "poweronrestart"))
dm_restart(DM_INIT_REASON_POWER_ON);
else if (!strcmp(argv[1], "inflightrestart"))
dm_restart(DM_INIT_REASON_IN_FLIGHT);
else
usage();
exit(1);
}
int test_dataman(int argc, char *argv[])
{
int i = 0;
unsigned num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1) {
num_tasks = atoi(argv[1]);
}
sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
task_returned_error = (bool *)calloc(num_tasks, sizeof(bool));
PX4_INFO("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
snprintf(a, 16, "%d", i);
char *av[] = {"tests_dataman", a, NULL};
px4_sem_init(sems + i, 1, 0);
/* sems use case is a signal */
px4_sem_setprotocol(sems, SEM_PRIO_NONE);
/* start the task */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
PX4_ERR("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
bool got_error = false;
for (i = 0; i < num_tasks; i++) {
if (task_returned_error[i]) {
got_error = true;
break;
}
}
free(task_returned_error);
if (got_error) {
return -1;
}
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_TEST; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_TEST) {
PX4_ERR("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_TEST; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
PX4_ERR("Restart power-on failed");
return -1;
}
}
return 0;
}
