fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 02:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix byte stuffing typo and add comments

Browse Source
This commit is contained in:
Mark Whitehorn
2016-01-24 11:22:18 +01:00
committed by Lorenz Meier
parent 8949a88f25
commit 82bb0564e2
2 changed files with 34 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/sPort_telemetry/sPort_data.c
+10 -4
View File
@@ -131,11 +131,11 @@ static void sPort_send_byte(int uart, uint8_t value)
const uint8_t x7D[] = { 0x7D, 0x5D };
switch (value) {
case 0x5E:
case 0x7E:
write(uart, x7E, sizeof(x7E));
break;
case 0x5D:
case 0x7D:
write(uart, x7D, sizeof(x7D));
break;
@@ -187,9 +187,15 @@ void sPort_send_A2(int uart)
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* 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);
/* send data for A2 */
// uint32_t voltage = (int)(255 * battery.voltage_v / 18.4f);
uint32_t voltage = (int)(255 * 5.5f / 18.4f);
uint32_t voltage = (int)(255 * vehicle_status.battery_voltage / 16.8f);
// static uint8_t counter=0;
// uint32_t voltage = counter++;
sPort_send_data(uart, 0xf103, voltage);
}
src/drivers/sPort_telemetry/sPort_telemetry.c
+24 -9
View File
@@ -182,21 +182,36 @@ static int sPort_telemetry_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
/* wait for poll frame starting with value 0x7E */
/* wait for poll frame starting with value 0x7E
* note that only the bus master is supposed to put a 0x7E on the bus.
* slaves use byte stuffing to send 0x7E and 0x7D.
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1);
if (status < 1) continue;
// read 2 bytes
int newBytes = read(uart, &sbuf[0], 2);
if (newBytes < 1 || sbuf[0] != 0x7E) continue;
if (status < 1) { continue; }
// read 1 byte
int newBytes = read(uart, &sbuf[0], 1);
if (newBytes < 1 || sbuf[0] != 0x7E) { continue; }
/* wait for ID byte */
status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1);
if (status < 1) { continue; }
newBytes = read(uart, &sbuf[1], 1);
// allow a minimum of 500usec before reply
usleep(500);
/* device ID 4 */
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));
}
}