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

Deamonized GPS app, fixed GPS issues, reworking RC input

This commit is contained in:
Lorenz Meier 2012-09-12 17:22:24 +02:00
parent 31ecc4d5df
commit c815aff842
5 changed files with 127 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
apps/gps/gps.c
View File

@ -58,6 +58,10 @@
#include <v1.0/common/mavlink.h>
#include <mavlink/mavlink_log.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* GPS module readout and publishing.
*
@ -68,6 +72,26 @@
*/
__EXPORT int gps_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int gps_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
exit(1);
}
/****************************************************************************
* Definitions
****************************************************************************/
@ -137,11 +161,56 @@ void setup_port(char *device, int speed, int *fd)
}
/*
* Main function of gps app.
/**
* The deamon 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 gps_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("gps already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)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("\gps is running\n");
} else {
printf("\tgps not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/*
* Main function of gps app.
*/
int gps_thread_main(int argc, char *argv[]) {
/* welcome message */
printf("[gps] Initialized. Searching for GPS receiver..\n");
@ -163,7 +232,7 @@ int gps_main(int argc, char *argv[])
/* read arguments */
int i;
for (i = 1; i < argc; i++) {
for (i = 0; i < argc; i++) {
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
printf(commandline_usage, argv[0]);
return 0;
@ -294,7 +363,7 @@ int gps_main(int argc, char *argv[])
}
while (true) {
while (!thread_should_exit) {
/* Infinite retries or break if retry == false */
/* Loop over all configurations of baud rate and protocol */
@ -482,7 +551,9 @@ int gps_main(int argc, char *argv[])
close(mavlink_fd);
return ERROR;
printf("[gps] exiting.\n");
return 0;
}
int open_port(char *port)

6
apps/gps/mtk.c
View File

@ -121,7 +121,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer)
if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) {
mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->alt = (int32_t)packet->msl_altitude * 10; // conversion from centimeters to millimeters, and from uint32_t to int16_t
mtk_gps->alt = (int32_t)(packet->msl_altitude * 10); // conversion from centimeters to millimeters, and from uint32_t to int16_t
mtk_gps->fix_type = packet->fix_type;
mtk_gps->eph = packet->hdop;
mtk_gps->epv = 65535; //unknown in mtk custom mode
@ -311,14 +311,14 @@ void *mtk_loop(void *arg)
/* advertise GPS topic */
struct vehicle_gps_position_s mtk_gps_d;
mtk_gps = &mtk_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
while (1) {
/* Parse a message from the gps receiver */
if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, &mtk_gps);
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
} else {
break;

4
apps/gps/nmea_helper.c
View File

@ -202,8 +202,8 @@ void *nmea_loop(void *arg)
nmea_gps->timestamp = hrt_absolute_time();
nmea_gps->time_gps_usec = epoch * 1e6 + info->utc.hsec * 1e4;
nmea_gps->fix_type = (uint8_t)info->fix;
nmea_gps->lat = (int32_t)ndeg2degree(info->lat) * 1e7;
nmea_gps->lon = (int32_t)ndeg2degree(info->lon) * 1e7;
nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7);
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7);
nmea_gps->alt = (int32_t)(info->elv * 1e3);
nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling

2
apps/mavlink/mavlink.c
View File

@ -1704,7 +1704,7 @@ int mavlink_main(int argc, char *argv[])
}
thread_should_exit = false;
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}

47
apps/px4/ground_estimator/ground_estimator.c
View File

@ -41,6 +41,9 @@
#include <unistd.h>
#include <stdio.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
static bool thread_should_exit = false; /**< ground_estimator exit flag */
static bool thread_running = false; /**< ground_estimator status flag */
static int ground_estimator_task; /**< Handle of ground_estimator task / thread */
@ -64,9 +67,49 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
printf("[ground_estimator] starting\n");
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
bool publishing = false;
/* advertise attitude */
struct debug_key_value_s dbg = { .key = "velx ", .value = 0.0f };
orb_advert_t pub_att = orb_advertise(ORB_ID(debug_key_value), &dbg);
float position[3] = {};
float velocity[3] = {};
uint64_t last_time = 0;
while (!thread_should_exit) {
printf("Hello ground_estimator!\n");
sleep(10);
/* wait for sensor update */
int ret = poll(fds, 1, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
} else {
struct sensor_combined_s s;
orb_copy(ORB_ID(sensor_combined), sub_raw, &s);
uint64_t dt = s.timestamp - last_time;
/* Integration */
position[0] += velocity[0] * dt;
position[1] += velocity[1] * dt;
position[2] += velocity[2] * dt;
velocity[0] += velocity[0] * 0.01f + 0.99f * s.acceleration_m_s2[0] * dt;
velocity[1] += velocity[1] * 0.01f + 0.99f * s.acceleration_m_s2[1] * dt;
velocity[2] += velocity[2] * 0.01f + 0.99f * s.acceleration_m_s2[2] * dt;
dbg.value = position[0];
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
}
}
printf("[ground_estimator] exiting.\n");