diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-06 15:47:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-06 15:47:34 +0200 |
commit | 1492323f0327916435d806c2af1e0c8296278c9d (patch) | |
tree | d53e2768c326ec6a2cbcfd896550c446ba93fab3 /src/drivers/hott/messages.cpp | |
parent | 2669f7f3af65921d4abbf3850cd62e48f2eeeec7 (diff) | |
parent | bd88951f6ce609bc5ba364bfa3d19ae61e444964 (diff) | |
download | px4-firmware-1492323f0327916435d806c2af1e0c8296278c9d.tar.gz px4-firmware-1492323f0327916435d806c2af1e0c8296278c9d.tar.bz2 px4-firmware-1492323f0327916435d806c2af1e0c8296278c9d.zip |
Merged master into uavcan
Diffstat (limited to 'src/drivers/hott/messages.cpp')
-rw-r--r-- | src/drivers/hott/messages.cpp | 34 |
1 files changed, 18 insertions, 16 deletions
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 90a744015..086132573 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -51,6 +51,8 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> +#include <drivers/drv_hrt.h> + /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 @@ -62,7 +64,6 @@ static int _airspeed_sub = -1; static int _esc_sub = -1; static orb_advert_t _esc_pub; -struct esc_status_s _esc; static bool _home_position_set = false; static double _home_lat = 0.0d; @@ -82,8 +83,6 @@ init_sub_messages(void) void init_pub_messages(void) { - memset(&_esc, 0, sizeof(_esc)); - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); } void @@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer) size_t size = sizeof(msg); memset(&msg, 0, size); memcpy(&msg, buffer, size); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + + // Publish it. + esc.timestamp = hrt_absolute_time(); + esc.esc_count = 1; + esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + esc.esc[0].esc_temperature = msg.temperature1 - 20; + esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { - orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + orb_publish(ORB_ID(esc_status), _esc_pub, &esc); } else { - _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } - - // Publish it. - _esc.esc_count = 1; - _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; - - _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; - _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - _esc.esc[0].esc_temperature = msg.temperature1 - 20; - _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); } void @@ -224,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - msg.gps_num_sat = gps.satellites_visible; + msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); |