diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 67336f01a2..58e0ddbacb 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -51,6 +51,7 @@ #include #include +#include #include @@ -58,9 +59,18 @@ #include #include +#include + #include "crtp.h" #include "syslink_main.h" +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); } @@ -74,7 +84,8 @@ Syslink::Syslink() : _battery_pub(nullptr), _rc_pub(nullptr), _cmd_pub(nullptr), - _rssi(RC_INPUT_RSSI_MAX) + _rssi(RC_INPUT_RSSI_MAX), + _bstate(BAT_DISCHARGING) { } @@ -130,6 +141,8 @@ Syslink::set_address(uint64_t addr) int count_out = 0; +static hrt_abstime lasttxtime = 0; // Last time a radio message was recieved + int Syslink::send_queued_raw_message() { @@ -137,6 +150,8 @@ Syslink::send_queued_raw_message() return 0; } + lasttxtime = hrt_absolute_time(); + syslink_message_t msg; msg.type = SYSLINK_RADIO_RAW; @@ -312,6 +327,8 @@ static int count_in = 0; //static int count_out = 0; static hrt_abstime lasttime = 0; +static hrt_abstime lastrxtime = 0; // Last time a radio message was recieved + void Syslink::handle_message(syslink_message_t *msg) @@ -337,13 +354,27 @@ Syslink::handle_message(syslink_message_t *msg) return; } - //uint8_t flags = msg->data[0]; - //float iset = *((float *)&msg->data[5]); - float vbat; + uint8_t flags = msg->data[0]; + int charging = flags & 1; + int powered = flags & 2; + + float vbat; //, iset; memcpy(&vbat, &msg->data[1], sizeof(float)); + //memcpy(&iset, &msg->data[5], sizeof(float)); _battery.updateBatteryStatus(t, vbat, -1, 0, false, &_battery_status); + + // Update battery charge state + if(charging) + _bstate = BAT_CHARGING; + /* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */ + else if(powered && !charging && _battery_status.voltage_filtered_v > 3.7f) + _bstate = BAT_CHARGED; + else + _bstate = BAT_DISCHARGING; + + // announce the battery status if needed, just publish else if (_battery_pub != nullptr) { orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); @@ -364,12 +395,39 @@ Syslink::handle_message(syslink_message_t *msg) } else if (msg->type == SYSLINK_RADIO_RAW) { handle_raw(msg); - + lastrxtime = t; } else { PX4_INFO("GOT %d", msg->type); } + float p = (t % 500000) / 500000.0f; + + /* Use LED_GREEN for charging indicator */ + if(_bstate == BAT_CHARGED) { + led_on(LED_GREEN); + } + else if(_bstate == BAT_CHARGING && p < 0.25f) { + led_on(LED_GREEN); + } + else + led_off(LED_GREEN); + + /* Alternate RX/TX LEDS when transfering */ + bool rx = t - lastrxtime < 200000, + tx = t - lasttxtime < 200000; + + + if(rx && p < 0.25f) + led_on(LED_RX); + else + led_off(LED_RX); + + if(tx && p > 0.5f && p > 0.75f) + led_on(LED_TX); + else + led_off(LED_TX); + } void diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h index b375253a49..aa491d8e06 100644 --- a/src/modules/syslink/syslink_main.h +++ b/src/modules/syslink/syslink_main.h @@ -43,6 +43,13 @@ #include "syslink.h" #include "crtp.h" +typedef enum { + BAT_DISCHARGING = 0, + BAT_CHARGING = 1, + BAT_CHARGED = 2 +} battery_state; + + class SyslinkBridge; class Syslink @@ -97,6 +104,7 @@ private: Battery _battery; int32_t _rssi; + battery_state _bstate; static int task_main_trampoline(int argc, char *argv[]);