dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Enable Q attitude estimator and INAV

This commit is contained in:
Lorenz Meier 2015-06-19 23:02:09 +02:00
parent f01913a2bc
commit cc499fcc29
5 changed files with 39 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/posix/config_posix_sitl.mk
View File

@ -31,6 +31,8 @@ MODULES += modules/mavlink
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/attitude_estimator_q
MODULES += modules/position_estimator_inav
#
# Vehicle Control

35
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -39,7 +39,7 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <px4_config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
@ -47,8 +47,6 @@
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
@ -189,7 +187,7 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@ -206,7 +204,7 @@ int AttitudeEstimatorQ::start() {
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2500,
(main_t)&AttitudeEstimatorQ::task_main_trampoline,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@ -224,7 +222,7 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
void AttitudeEstimatorQ::task_main() {
warnx("started");
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@ -431,46 +429,53 @@ void AttitudeEstimatorQ::update(float dt) {
int attitude_estimator_q_main(int argc, char *argv[]) {
if (argc < 1) {
errx(1, "usage: attitude_estimator_q {start|stop|status}");
warnx("usage: attitude_estimator_q {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (attitude_estimator_q::instance != nullptr) {
errx(1, "already running");
warnx("already running");
return 1;
}
attitude_estimator_q::instance = new AttitudeEstimatorQ;
if (attitude_estimator_q::instance == nullptr) {
errx(1, "alloc failed");
warnx("alloc failed");
return 1;
}
if (OK != attitude_estimator_q::instance->start()) {
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
err(1, "start failed");
warnx("start failed");
return 1;
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (attitude_estimator_q::instance == nullptr) {
errx(1, "not running");
warnx("not running");
return 1;
}
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (attitude_estimator_q::instance) {
errx(0, "running");
warnx("running");
return 0;
} else {
errx(1, "not running");
warnx("not running");
return 1;
}
}

24
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -45,7 +45,6 @@
#include <fcntl.h>
#include <string.h>
#include <px4_config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <math.h>
@ -91,7 +90,6 @@ static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
static const uint32_t updates_counter_len = 1000000;
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
@ -121,7 +119,7 @@ static void usage(const char *reason)
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
return;
}
/**
@ -142,7 +140,7 @@ int position_estimator_inav_main(int argc, char *argv[])
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
return 0;
}
verbose_mode = false;
@ -157,7 +155,7 @@ int position_estimator_inav_main(int argc, char *argv[])
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
position_estimator_inav_thread_main,
(argv) ? (char * const *) &argv[2] : (char * const *) NULL);
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
@ -169,7 +167,7 @@ int position_estimator_inav_main(int argc, char *argv[])
warnx("not started");
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
@ -180,11 +178,11 @@ int position_estimator_inav_main(int argc, char *argv[])
warnx("not started");
}
exit(0);
return 0;
}
usage("unrecognized command");
exit(1);
return 1;
}
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
@ -194,7 +192,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
hrt_absolute_time(), msg, (double)dt,
(unsigned long long)hrt_absolute_time(), msg, (double)dt,
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
fwrite(s, 1, n, f);
@ -348,13 +346,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
parameters_init(&pos_inav_param_handles);
inav_parameters_init(&pos_inav_param_handles);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
/* first parameters update */
parameters_update(&pos_inav_param_handles, &params);
inav_parameters_update(&pos_inav_param_handles, &params);
struct pollfd fds_init[1] = {
{ .fd = sensor_combined_sub, .events = POLLIN },
@ -428,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&pos_inav_param_handles, &params);
inav_parameters_update(&pos_inav_param_handles, &params);
}
/* actuator */

8
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -301,7 +301,7 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
*/
PARAM_DEFINE_INT32(INAV_ENABLED, 1);
int parameters_init(struct position_estimator_inav_param_handles *h)
int inav_parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
@ -326,10 +326,10 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->no_vision = param_find("CBRK_NO_VISION");
h->delay_gps = param_find("INAV_DELAY_GPS");
return OK;
return 0;
}
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
@ -353,5 +353,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->no_vision, &(p->no_vision));
param_get(h->delay_gps, &(p->delay_gps));
return OK;
return 0;
}

4
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -97,10 +97,10 @@ struct position_estimator_inav_param_handles {
* Initialize all parameter handles and values
*
*/
int parameters_init(struct position_estimator_inav_param_handles *h);
int inav_parameters_init(struct position_estimator_inav_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);