fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

add sPort GPS telemetry

Browse Source
This commit is contained in:
Mark Whitehorn 2016-02-24 21:19:30 -07:00 committed by Lorenz Meier
parent 584165def2
commit 6fa6e72250
3 changed files with 119 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/drivers/frsky_telemetry/frsky_telemetry.c
View File

@ -268,6 +268,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
static hrt_abstime lastSPD = 0;
static hrt_abstime lastFUEL = 0;
static hrt_abstime lastVSPD = 0;
static hrt_abstime lastGPS = 0;
switch (sbuf[1]) {
@ -344,6 +345,44 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
sPort_send_VSPD(uart, speed);
}
break;
case SMARTPORT_POLL_7:
/* report GPS data elements at 2*5Hz */
if (now - lastGPS > 100 * 1000) {
static int elementCount = 0;
switch (elementCount) {
case 0:
sPort_send_GPS_LON(uart);
elementCount++;
break;
case 1:
sPort_send_GPS_LAT(uart);
elementCount++;
break;
case 2:
sPort_send_GPS_COG(uart);
elementCount++;
break;
case 3:
sPort_send_GPS_ALT(uart);
elementCount++;
break;
case 4:
sPort_send_GPS_SPD(uart);
elementCount = 0;
break;
}
}
break;
}
}

94
src/drivers/frsky_telemetry/sPort_data.c
View File

@ -58,31 +58,26 @@
#define frac(f) (f - (int)f)
static int battery_sub = -1;
//static int battery_sub = -1;
static int sensor_sub = -1;
static int global_position_sub = -1;
static int vehicle_status_sub = -1;
static struct vehicle_status_s vehicle_status;
static struct sensor_combined_s sensor_data;
static struct vehicle_global_position_s global_pos;
/**
* Initializes the uORB subscriptions.
*/
void sPort_init()
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
// battery_sub = orb_subscribe(ORB_ID(battery_status));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
/**
* Sends a 0x10 start byte.
*/
//static void sPort_send_start(int uart)
//{
// static const uint8_t c = 0x10;
// write(uart, &c, 1);
//}
static void update_crc(uint16_t *crc, unsigned char b)
{
*crc += b;
@ -152,7 +147,6 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
void sPort_send_BATV(int uart)
{
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
@ -165,7 +159,6 @@ void sPort_send_BATV(int uart)
void sPort_send_CUR(int uart)
{
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
@ -180,12 +173,11 @@ void sPort_send_CUR(int uart)
void sPort_send_ALT(int uart)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
memset(&sensor_data, 0, sizeof(sensor_data));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_data);
/* send data */
uint32_t alt = (int)(100 * raw.baro_alt_meter[0]);
uint32_t alt = (int)(100 * sensor_data.baro_alt_meter[0]);
sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
}
@ -193,11 +185,10 @@ void sPort_send_ALT(int uart)
void sPort_send_SPD(int uart)
{
/* get a local copy of the global position data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* send data for A2 */
/* send data */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint32_t ispeed = (int)(10 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
@ -215,7 +206,6 @@ void sPort_send_VSPD(int uart, float speed)
void sPort_send_FUEL(int uart)
{
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
@ -223,3 +213,67 @@ void sPort_send_FUEL(int uart)
uint32_t fuel = (int)(100 * vehicle_status.battery_remaining);
sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
}
void sPort_send_GPS_LON(int uart)
{
/* send longitude */
/* get a local copy of the global position data */
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
/* precision is approximately 0.1m */
uint32_t iLon = 6E5 * fabs(global_pos.lon);
iLon |= (1 << 31);
if (global_pos.lon < 0) { iLon |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
}
void sPort_send_GPS_LAT(int uart)
{
/* send latitude */
/* get a local copy of the global position data */
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
uint32_t iLat = 6E5 * fabs(global_pos.lat);
if (global_pos.lat < 0) { iLat |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
}
void sPort_send_GPS_ALT(int uart)
{
/* send altitude */
/* get a local copy of the global position data */
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* convert to 100 * m/sec */
uint32_t iAlt = 100 * global_pos.alt;
sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
}
void sPort_send_GPS_COG(int uart)
{
/* send course */
/* get a local copy of the global position data */
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
uint32_t iYaw = 100 * global_pos.yaw;
sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
}
void sPort_send_GPS_SPD(int uart)
{
/* get a local copy of the global position data */
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* send 100 * knots */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint32_t ispeed = (int)(1944 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}

7
src/drivers/frsky_telemetry/sPort_data.h
View File

@ -51,6 +51,7 @@
#define SMARTPORT_POLL_4 0x16
#define SMARTPORT_POLL_5 0xB7
#define SMARTPORT_POLL_6 0x00
#define SMARTPORT_POLL_7 0x83
/* FrSky SmartPort sensor IDs */
#define SMARTPORT_ID_RSSI 0xf101
@ -84,7 +85,11 @@ void sPort_send_CUR(int uart);
void sPort_send_ALT(int uart);
void sPort_send_SPD(int uart);
void sPort_send_VSPD(int uart, float speed);
void sPort_send_GPS_LON(int uart);
void sPort_send_GPS_LAT(int uart);
void sPort_send_GPS_ALT(int uart);
void sPort_send_GPS_COG(int uart);
void sPort_send_GPS_SPD(int uart);
void sPort_send_FUEL(int uart);
#endif /* _SPORT_TELEMETRY_H */