mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 14:08:52 +08:00
Completed main implementation and debugging
This commit is contained in:
Simon Wilks
@@ -138,6 +138,7 @@ recv_req_id(int uart, uint8_t *id)
|
||||
|
||||
/* if we have a binary mode request */
|
||||
if (mode != BINARY_MODE_REQUEST_ID) {
|
||||
warnx("Non binary request ID detected: %d", mode);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -232,6 +233,8 @@ hott_telemetry_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
send_data(uart, buffer, size);
|
||||
} else {
|
||||
warnx("NOK");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -40,8 +40,9 @@
|
||||
#include "messages.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <unistd.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
@@ -51,11 +52,17 @@
|
||||
/* The board is very roughly 5 deg warmer than the surrounding air */
|
||||
#define BOARD_TEMP_OFFSET_DEG 5
|
||||
|
||||
#define ALT_OFFSET 500.0f
|
||||
|
||||
static int battery_sub = -1;
|
||||
static int gps_sub = -1;
|
||||
static int home_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
|
||||
static bool home_position_set = false;
|
||||
static double home_lat = 0.0d;
|
||||
static double home_lon = 0.0d;
|
||||
|
||||
void
|
||||
messages_init(void)
|
||||
{
|
||||
@@ -69,15 +76,18 @@ void
|
||||
build_eam_response(uint8_t *buffer, size_t *size)
|
||||
{
|
||||
/* get a local copy of the current sensor values */
|
||||
struct sensor_combined_s raw = { 0 };
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
/* get a local copy of the battery data */
|
||||
struct battery_status_s battery = { 0 };
|
||||
struct battery_status_s battery;
|
||||
memset(&battery, 0, sizeof(battery));
|
||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
||||
|
||||
struct eam_module_msg msg = { 0 };
|
||||
struct eam_module_msg msg;
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
msg.start = START_BYTE;
|
||||
msg.eam_sensor_id = EAM_SENSOR_ID;
|
||||
@@ -92,12 +102,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
// TODO: flight time
|
||||
// TODO: climb rate
|
||||
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
|
||||
@@ -105,102 +110,113 @@ void
|
||||
build_gps_response(uint8_t *buffer, size_t *size)
|
||||
{
|
||||
/* get a local copy of the current sensor values */
|
||||
struct sensor_combined_s raw = { 0 };
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
/* get a local copy of the battery data */
|
||||
struct vehicle_gps_position_s gps = { 0 };
|
||||
struct vehicle_gps_position_s gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
|
||||
|
||||
struct gps_module_msg msg = { 0 };
|
||||
*size = sizeof(msg);
|
||||
memset(&msg, 0, *size);
|
||||
|
||||
msg.start = START_BYTE;
|
||||
msg.sensor_id = GPS_SENSOR_ID;
|
||||
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
|
||||
|
||||
/* Current flight direction */
|
||||
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
|
||||
|
||||
/* GPS speed */
|
||||
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
|
||||
msg.gps_speed_L = (uint8_t)speed & 0xff;
|
||||
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
/* Get latitude in degrees, minutes and seconds */
|
||||
double lat = ((double)(gps.lat)) * 1e-7d;
|
||||
|
||||
msg.latitude_ns = 0;
|
||||
if (lat < 0) {
|
||||
msg.latitude_ns = 1;
|
||||
lat = -lat;
|
||||
}
|
||||
|
||||
int deg;
|
||||
int min;
|
||||
int sec;
|
||||
convert_to_degrees_minutes_seconds(lat, °, &min, &sec);
|
||||
|
||||
uint16_t lat_min = (uint16_t)(deg * 100 + min);
|
||||
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
|
||||
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
|
||||
uint16_t lat_sec = (uint16_t)(sec);
|
||||
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
|
||||
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
|
||||
|
||||
|
||||
/* Get longitude in degrees, minutes and seconds */
|
||||
double lon = ((double)(gps.lon)) * 1e-7d;
|
||||
|
||||
msg.longitude_ew = 0;
|
||||
if (lon < 0) {
|
||||
msg.longitude_ew = 1;
|
||||
lon = -lon;
|
||||
}
|
||||
|
||||
convert_to_degrees_minutes_seconds(lon, °, &min, &sec);
|
||||
|
||||
uint16_t lon_min = (uint16_t)(deg * 100 + min);
|
||||
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
|
||||
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
|
||||
uint16_t lon_sec = (uint16_t)(sec);
|
||||
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
|
||||
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
|
||||
|
||||
/* Altitude */
|
||||
uint16_t alt = (uint16_t)(gps.alt * 1e-3 + 500.0f);
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
/* Distance from home */
|
||||
bool updated;
|
||||
orb_check(home_sub, &updated);
|
||||
if (updated) {
|
||||
/* get a local copy of the home position data */
|
||||
struct home_position_s home = { 0 };
|
||||
orb_copy(ORB_ID(home_position), home_sub, &home);
|
||||
|
||||
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(
|
||||
(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon);
|
||||
warnx("dist %d home.lat %3.6f home.lon %3.6f lat %3.6f lon %3.6f ",
|
||||
dist, (double)home.lat*1e-7d, (double)home.lon*1e-7d, lat, lon);
|
||||
msg.distance_L = (uint8_t)dist & 0xff;
|
||||
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
|
||||
|
||||
/* Direction back to home */
|
||||
uint16_t bearing = (uint16_t)get_bearing_to_next_waypoint(
|
||||
(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon) * M_RAD_TO_DEG_F;
|
||||
msg.home_direction = (uint8_t)bearing >> 1;
|
||||
}
|
||||
|
||||
msg.gps_num_sat = gps.satellites_visible;
|
||||
|
||||
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
|
||||
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
|
||||
msg.gps_fix = (uint8_t)(gps.fix_type + 48);
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
/* No point collecting more data if we don't have a 3D fix yet */
|
||||
if (gps.fix_type > 2) {
|
||||
/* Current flight direction */
|
||||
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
|
||||
|
||||
/* GPS speed */
|
||||
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
|
||||
msg.gps_speed_L = (uint8_t)speed & 0xff;
|
||||
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
/* Get latitude in degrees, minutes and seconds */
|
||||
double lat = ((double)(gps.lat))*1e-7d;
|
||||
|
||||
/* Prepend N or S specifier */
|
||||
msg.latitude_ns = 0;
|
||||
if (lat < 0) {
|
||||
msg.latitude_ns = 1;
|
||||
lat = abs(lat);
|
||||
}
|
||||
|
||||
int deg;
|
||||
int min;
|
||||
int sec;
|
||||
convert_to_degrees_minutes_seconds(lat, °, &min, &sec);
|
||||
|
||||
uint16_t lat_min = (uint16_t)(deg * 100 + min);
|
||||
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
|
||||
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
|
||||
uint16_t lat_sec = (uint16_t)(sec);
|
||||
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
|
||||
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
|
||||
|
||||
/* Get longitude in degrees, minutes and seconds */
|
||||
double lon = ((double)(gps.lon))*1e-7d;
|
||||
|
||||
/* Prepend E or W specifier */
|
||||
msg.longitude_ew = 0;
|
||||
if (lon < 0) {
|
||||
msg.longitude_ew = 1;
|
||||
lon = abs(lon);
|
||||
}
|
||||
|
||||
convert_to_degrees_minutes_seconds(lon, °, &min, &sec);
|
||||
|
||||
uint16_t lon_min = (uint16_t)(deg * 100 + min);
|
||||
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
|
||||
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
|
||||
uint16_t lon_sec = (uint16_t)(sec);
|
||||
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
|
||||
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
|
||||
|
||||
/* Altitude */
|
||||
uint16_t alt = (uint16_t)(gps.alt*1e-3 + ALT_OFFSET );
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
/* Get any (and probably only ever one) home_sub postion report */
|
||||
bool updated;
|
||||
orb_check(home_sub, &updated);
|
||||
if (updated) {
|
||||
/* get a local copy of the home position data */
|
||||
struct home_position_s home;
|
||||
memset(&home, 0, sizeof(home));
|
||||
orb_copy(ORB_ID(home_position), home_sub, &home);
|
||||
|
||||
home_lat = ((double)(home.lat))*1e-7d;
|
||||
home_lon = ((double)(home.lon))*1e-7d;
|
||||
home_position_set = true;
|
||||
}
|
||||
|
||||
/* Distance from home */
|
||||
if (home_position_set) {
|
||||
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
|
||||
|
||||
msg.distance_L = (uint8_t)dist & 0xff;
|
||||
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
|
||||
|
||||
/* Direction back to home */
|
||||
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
|
||||
msg.home_direction = (uint8_t)bearing >> 1;
|
||||
}
|
||||
}
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
|
||||
@@ -210,6 +226,8 @@ convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
|
||||
*deg = (int)val;
|
||||
|
||||
double delta = val - *deg;
|
||||
*min = (int)(delta * 60.0);
|
||||
*sec = (int)(delta * 3600.0);
|
||||
const double min_d = delta * 60.0d;
|
||||
*min = (int)min_d;
|
||||
delta = min_d - *min;
|
||||
*sec = (int)(delta * 10000.0d);
|
||||
}
|
||||
|
||||
@@ -189,6 +189,7 @@ struct gps_module_msg {
|
||||
void messages_init(void);
|
||||
void build_eam_response(uint8_t *buffer, size_t *size);
|
||||
void build_gps_response(uint8_t *buffer, size_t *size);
|
||||
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
||||
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user