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

fix CRC calc and add battery voltage report

This commit is contained in:
Mark Whitehorn 2016-01-18 20:01:52 -07:00 committed by Lorenz Meier
parent ff690dda80
commit 8949a88f25
3 changed files with 33 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/drivers/sPort_telemetry/sPort_data.c
View File

@ -109,13 +109,13 @@ void sPort_init()
/**
* Sends a 0x10 start byte.
*/
static void sPort_send_start(int uart)
{
static const uint8_t c = 0x10;
write(uart, &c, 1);
}
//static void sPort_send_start(int uart)
//{
// static const uint8_t c = 0x10;
// write(uart, &c, 1);
//}
static void update_crc(uint16_t *crc, uint8_t b)
static void update_crc(uint16_t *crc, unsigned char b)
{
*crc += b;
*crc += *crc >> 8;
@ -155,9 +155,12 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
uint8_t byte[4];
} buf;
uint16_t crc = 0;
sPort_send_start(uart);
// write(uart, 0x10, 1);
/* send start byte */
static const uint8_t c = 0x10;
write(uart, &c, 1);
/* init crc */
uint16_t crc = c;
buf.word = id;
@ -173,7 +176,21 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
sPort_send_byte(uart, buf.byte[i]); /* LSB first */
}
sPort_send_byte(uart, crc);
sPort_send_byte(uart, 0xFF - crc);
}
void sPort_send_A2(int uart)
{
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* send data for A2 */
// uint32_t voltage = (int)(255 * battery.voltage_v / 18.4f);
uint32_t voltage = (int)(255 * 5.5f / 18.4f);
sPort_send_data(uart, 0xf103, voltage);
}
#ifdef xxxx

1
src/drivers/sPort_telemetry/sPort_data.h
View File

@ -47,5 +47,6 @@
// Public functions
void sPort_init(void);
void sPort_send_data(int uart, uint16_t id, uint32_t data);
void sPort_send_A2(int uart);
#endif /* _SPORT_TELEMETRY_H */

10
src/drivers/sPort_telemetry/sPort_telemetry.c
View File

@ -191,12 +191,12 @@ static int sPort_telemetry_thread_main(int argc, char *argv[])
if (newBytes < 1 || sbuf[0] != 0x7E) continue;
/* device ID 4 */
static uint8_t counter = 0;
if (sbuf[1] == 0xe4) {
/* send data for A2 */
sPort_send_data(uart, 0xf103, counter++);
if (sbuf[1] == 0x1B) {
/* send battery voltage */
sPort_send_A2(uart);
/* read it back */
read(uart, &sbuf[0], sizeof(sbuf));
read(uart, &sbuf[0], sizeof(sbuf));
}
}