2016-04-11 04:15:50 +02:00
|
|
|
|
|
|
|
|
2018-03-03 17:42:28 +01:00
|
|
|
#include "mbag.h"
|
2016-04-11 04:15:50 +02:00
|
|
|
#include "action.h"
|
|
|
|
#include "cw.h"
|
2016-04-18 07:40:30 +02:00
|
|
|
#include "cw/dbg.h"
|
2016-04-11 04:15:50 +02:00
|
|
|
|
|
|
|
|
|
|
|
int cw_out_radio_generic(struct conn *conn, struct cw_action_out *a, uint8_t * dst)
|
|
|
|
{
|
2016-04-18 07:40:30 +02:00
|
|
|
cw_dbg(DBG_X,"Radio Generic out %s",a->item_id);
|
|
|
|
|
2016-04-11 04:15:50 +02:00
|
|
|
int l=0;
|
|
|
|
MAVLITER_DEFINE(it,conn->radios_upd);
|
|
|
|
mavliter_foreach(&it){
|
2016-04-11 08:37:52 +02:00
|
|
|
struct mbag_item *radio = mavliter_get(&it);
|
|
|
|
if ( radio->type != MBAG_MBAG ) {
|
2016-04-11 04:15:50 +02:00
|
|
|
continue;
|
|
|
|
}
|
2016-04-11 08:37:52 +02:00
|
|
|
|
|
|
|
/* Size for msg elem header depends on
|
|
|
|
vendor specific payload */
|
|
|
|
int start = a->vendor_id ? 10 : 4;
|
|
|
|
|
2016-04-12 07:37:27 +02:00
|
|
|
uint8_t * d = dst+l;
|
|
|
|
|
2016-04-11 08:37:52 +02:00
|
|
|
|
|
|
|
|
2018-03-03 17:42:28 +01:00
|
|
|
struct mbag_item * item = mbag_get(radio->u2.data,a->item_id);
|
2016-04-11 08:37:52 +02:00
|
|
|
if (!item){
|
2018-03-03 17:42:28 +01:00
|
|
|
cw_dbg(DBG_X,"Not found! %s for rid %d",a->item_id,radio->u1.iid);
|
2016-04-11 08:37:52 +02:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
int len=0;
|
2018-03-03 17:42:28 +01:00
|
|
|
len += cw_put_byte(d+start,radio->u1.iid);
|
2016-04-18 07:40:30 +02:00
|
|
|
|
2018-03-03 17:42:28 +01:00
|
|
|
cw_dbg(DBG_X, "Radio generic out '%s' fro rid %d",a->item_id,radio->u1.iid);
|
2016-04-12 07:37:27 +02:00
|
|
|
len += cw_put_mbag_item(d + start+1, item);
|
2016-04-11 08:37:52 +02:00
|
|
|
|
|
|
|
if (a->vendor_id)
|
2016-04-12 07:37:27 +02:00
|
|
|
l+= len + cw_put_elem_vendor_hdr(d, a->vendor_id, a->elem_id, len);
|
2016-04-11 08:37:52 +02:00
|
|
|
else
|
2016-04-12 07:37:27 +02:00
|
|
|
l += len + cw_put_elem_hdr(d, a->elem_id, len);
|
2016-04-11 08:37:52 +02:00
|
|
|
|
|
|
|
|
2016-04-11 04:15:50 +02:00
|
|
|
|
|
|
|
}
|
|
|
|
return l;
|
|
|
|
|
|
|
|
}
|