/* Returns error (0 = no error, otherwise error occurred) */
extern int nmt_slave_send_heartbeat(co_node *node, uint32_t tick_count)
{
int error = 0;
// Check object dictionary to see if heartbeat is enabled and if so what is it's frequency
// The value is found in OD index 0x1017, sub index 0, data type uint16
uint32_t interval;
od_result result = od_read(node->od, 0x1017, 0, &interval);
if (result == OD_RESULT_OK)
{
if (interval != 0)
{
if (tick_count - previous_heartbeat_tick >= interval)
{
send_heartbeat(node);
previous_heartbeat_tick = tick_count;
}
}
}
else
{
log_write_ln("nmt_slave: ERROR: reading heartbeat OD entry failed");
error = 1;
}
return error;
}
void Heartbeat::init(std::string id)
{
void* zmq_ctx = g_zmq_ctx.get_zmq_ctx();
gate_ = zmq_socket(zmq_ctx, ZMQ_PAIR);
if (zmq_connect(gate_, MAGIC_GATE))
{
zmq_strerror(errno);
}
myid_ = id;
ios_ptr_ = &g_proactor.get_ios();
udp_socket_.reset(new boost::asio::ip::udp::socket(*ios_ptr_));
strand_.reset(new boost::asio::strand(*ios_ptr_));
strand4peermap_.reset(new boost::asio::strand(*ios_ptr_));
heartbeat_timer_.reset(new boost::asio::deadline_timer(*ios_ptr_, boost::posix_time::milliseconds(HEARTBEAT_INTERVAL)));
broadcast_addr_ = boost::asio::ip::address_v4::broadcast();
broadcast_ep_ = boost::asio::ip::udp::endpoint(broadcast_addr_, HEARTBEAT_PORT);
udp_socket_->open(boost::asio::ip::udp::v4());
boost::asio::ip::udp::socket::broadcast broadcast_option(true);
boost::asio::ip::udp::socket::reuse_address reuse_option(true);
boost::asio::ip::multicast::enable_loopback loopback_option(true);
udp_socket_->set_option(reuse_option);
udp_socket_->set_option(broadcast_option);
udp_socket_->set_option(loopback_option);
udp_socket_->bind(broadcast_ep_);
strand_->post(boost::bind(&Heartbeat::start_receive, this));
send_heartbeat();
}
static void
sig_heartbeat(int signum)
{
send_heartbeat(signum);
check_heartbeat(signum);
alarm(NODE_HEARTBEAT_INTERVAL);
}
/******************************************************************************
* *
* Function: main_heart_loop *
* *
* Purpose: periodically send heartbeat message to the server *
* *
* Parameters: *
* *
* Return value: *
* *
* Author: Alexander Vladishev *
* *
* Comments: *
* *
******************************************************************************/
void main_heart_loop(void)
{
int start, sleeptime = 0, res;
double sec, total_sec = 0.0, old_total_sec = 0.0;
time_t last_stat_time;
#define STAT_INTERVAL 5 /* if a process is busy and does not sleep then update status not faster than */
/* once in STAT_INTERVAL seconds */
last_stat_time = time(NULL);
zbx_setproctitle("%s [sending heartbeat message]", get_process_type_string(process_type));
for (;;)
{
if (0 != sleeptime)
{
zbx_setproctitle("%s [sending heartbeat message %s in " ZBX_FS_DBL " sec, "
"sending heartbeat message]",
get_process_type_string(process_type),
SUCCEED == res ? "success" : "failed", old_total_sec);
}
start = time(NULL);
sec = zbx_time();
res = send_heartbeat();
total_sec += zbx_time() - sec;
sleeptime = CONFIG_HEARTBEAT_FREQUENCY - (time(NULL) - start);
if (0 != sleeptime || STAT_INTERVAL <= time(NULL) - last_stat_time)
{
if (0 == sleeptime)
{
zbx_setproctitle("%s [sending heartbeat message %s in " ZBX_FS_DBL " sec, "
"sending heartbeat message]",
get_process_type_string(process_type),
SUCCEED == res ? "success" : "failed", total_sec);
}
else
{
zbx_setproctitle("%s [sending heartbeat message %s in " ZBX_FS_DBL " sec, "
"idle %d sec]",
get_process_type_string(process_type),
SUCCEED == res ? "success" : "failed", total_sec, sleeptime);
old_total_sec = total_sec;
}
total_sec = 0.0;
last_stat_time = time(NULL);
}
zbx_sleep_loop(sleeptime);
}
#undef STAT_INTERVAL
}
void test_schedule(){
int del = 60; // 1 min
sleep(del*10);
send_buzz();
sleep(del*14);
send_heartbeat();
sleep(del*8);
send_hug();
sleep(del*14);
send_heartbeat();
sleep(del*20);
send_hug();
}
static void heartbeat_timeout_callback(int fd, short event, void *arg) {
struct server_context_t *s = (struct server_context_t *)arg;
if(event & EV_TIMEOUT) {
DBG_LOG(LOG_DEBUG, "[%s][%d] Heartbeat timeout occurred", ss[s->state], s->current_term);
if(s->state == LEADER) {
//heartbeat timout occured -- send empty append_entries
send_heartbeat(s);
}
}
}
void MAVLinkMKApp::handle_input(const mk_message_t& msg) {
uint64_t tmp64 = message_time;
message_time = get_time_us();
log("MAVLinkMKApp got mk_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);
//send heartbeat approx. after 1s
delta_time += message_time - tmp64;
if( delta_time > 1000000 ) {
send_heartbeat();
delta_time = 0;
}
//TODO: check coded
//TODO: check size of msg
switch(msg.type) {
case MK_MSG_TYPE_DEBUGOUT:
break;
case MK_MSG_TYPE_EXT_CTRL: {
mavlink_manual_control_t manual_control;
copy(&manual_control, (mk_extern_control_t*)msg.data);
Lock tx_mav_lock(tx_mav_mutex);
mavlink_msg_manual_control_encode(system_id(),
component_id,
&tx_mav_msg,
&manual_control);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MK_MSG_TYPE_COMPASS:
break;
case MK_MSG_TYPE_POLL_DEBUG:
break;
case MK_MSG_TYPE_ANGLE_OUT: {
mavlink_debug_t debug;
debug.ind = 0;
mk_angles_t *mk_angles = (mk_angles_t*)msg.data;
debug.value = ((float)mk_angles->pitch) / 10;
Lock tx_mav_lock(tx_mav_mutex);
mavlink_msg_debug_encode(system_id(),
component_id,
&tx_mav_msg,
&debug);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MK_MSG_TYPE_MOTORTEST:
break;
case MK_MSG_TYPE_SETTING:
break;
default:
break;
}
}
void debug_schedule(){
int del = 2; // 2 sek
sleep(del);
send_hug();
sleep(del);
send_heartbeat();
sleep(del);
send_buzz();
}
void broker_t::renew_recipients() {
auto recipient_iterator = m_recipients.begin();
while(recipient_iterator != m_recipients.end()) {
recipient_ptr_t& recipient = recipient_iterator->second;
if(recipient->expired() || recipient->disconnected()) {
m_recipients.erase(recipient_iterator++);
}
else {
send_heartbeat(recipient);
++recipient_iterator;
}
}
}
bool try_send_message(mavlink_channel_t chan, int msgid) {
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
switch (msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
default:
return false;
}
}
static void request_vote_cb(struct data_t *result, char *err, void *data) {
struct server_context_t *s = (struct server_context_t *)data;
if(err) {
//what should we do with bysentine errors?
//LOG: fatal error. TODO: exit?
return;
}
if(s->state != CANDIDATE) {
return;
}
struct request_vote_output_t *vote = get_request_vote_output(result);
if(!vote) {
log_fatal_and_exit(s,
"RequestVoteCallback: Response parsing failed");
return;
}
int pair_vote = vote->vote_granted;
uint64_t pair_term = vote->term;
free(vote);
if(pair_vote && pair_term == s->current_term
&& s->state == CANDIDATE) {
s->current_votes++;
} else {
if(pair_term > s->current_term) {
set_term(s, pair_term);
s->voted_for = 0;
save_state(s);
}
return;
}
int votes_needed = s->quoram_size/2 + 1;
if(s->current_votes == votes_needed) {
s->state = LEADER;
s->current_leader = s->id;
DBG_LOG(LOG_INFO, "[%s][%d] Server is elected as the leader", ss[s->state], s->current_term);
save_state(s);
//reset timer such that election timeout does not occur
//and heartbeat timeout occurs
reset_timers(s, false, true);
for(int i = 0; i < s->quoram_size - 1; i++) {
if(s->last_entry) {
s->next_index[i] = s->last_entry->index + 1;
} else {
s->next_index[i] = 1;
}
s->match_index[i] = 0;
}
send_heartbeat(s);
}
}
void Pixhawk_Interface::
heartbeat_thread()
{
printf("hi134\n");
int time = 0;
while (1)
{
printf("h21afsfsi\n");
mavlink_message_t msg;
send_heartbeat(msg);
printf("<3 time: %d ms\n", millis()-time);
time = millis();
delay(1000);
}
}
int Gate_Client_Messager::process_gate_block(int cid, int msg_id, Block_Buffer &buf) {
int ret = 0;
switch (msg_id) {
case REQ_CONNECT_GATE: {
connect_gate(cid, buf);
break;
}
case REQ_SEND_HEARTBEAT: {
send_heartbeat(cid, buf);
break;
}
default:
break;
}
return ret;
}
/******************************************************************************
* *
* Function: main_heart_loop *
* *
* Purpose: periodically send heartbeat message to the server *
* *
* Parameters: *
* *
* Return value: *
* *
* Author: Alexander Vladishev *
* *
* Comments: *
* *
******************************************************************************/
void main_heart_loop()
{
int start, sleeptime;
zabbix_log(LOG_LEVEL_DEBUG, "In main_heart_loop()");
for (;;)
{
start = time(NULL);
zbx_setproctitle("%s [sending heartbeat message]", get_process_type_string(process_type));
send_heartbeat();
sleeptime = CONFIG_HEARTBEAT_FREQUENCY - (time(NULL) - start);
zbx_sleep_loop(sleeptime);
}
}
void DropletCustomOne::change_state ( State new_state )
{
state = new_state;
switch ( state )
{
case COLLABORATING:
set_rgb_led ( 0, 0, 250 );
collab_time = get_32bit_time ();
break;
case LEAVING:
set_rgb_led ( 0, 250, 250 );
move_duration ( (last_move_dir + 3) % 6, WALKAWAY_TIME );
case SAFE:
set_rgb_led ( 255, 255, 0 );
break;
case START_DELAY:
start_delay_time = get_32bit_time ();
break;
case WAITING:
cancel_move ();
unique_ids.clear ();
set_rgb_led ( 0, 255, 0 );
// Clear out all messages in buffer first.
while ( check_for_new_messages() );
heartbeat_time = get_32bit_time ();
voting_time = get_32bit_time ();
send_heartbeat ();
break;
case SEARCHING:
default:
reset_rgb_led ();
}
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int16_t payload_space = serial_free();
if (telemetry_delayed(chan)) {
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(chan);
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
default:
break;
}
return true;
}
void MAVLinkMKHUCHApp::handle_input(const mkhuch_message_t& msg) {
uint64_t last_mkhuch_msg_time = mkhuch_msg_time;
mkhuch_msg_time = get_time_us();
log("MAVLinkMKHUCHApp got mkhuch_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);
//send heartbeat approx. after 1s
heartbeat_time += mkhuch_msg_time - last_mkhuch_msg_time;
if( heartbeat_time > 1000000 ) {
send_heartbeat();
heartbeat_time = 0;
}
switch(msg.type) {
case MKHUCH_MSG_TYPE_MK_IMU: {
const mkhuch_mk_imu_t *mk_imu = reinterpret_cast<const mkhuch_mk_imu_t*>(msg.data);
mavlink_huch_imu_raw_adc_t imu_raw_adc;
imu_raw_adc.xacc = mk_imu->x_adc_acc;
imu_raw_adc.yacc = mk_imu->y_adc_acc;
imu_raw_adc.zacc = mk_imu->z_adc_acc;
imu_raw_adc.xgyro = mk_imu->x_adc_gyro;
imu_raw_adc.ygyro = mk_imu->y_adc_gyro;
imu_raw_adc.zgyro = mk_imu->z_adc_gyro;
DataCenter::set_huch_imu_raw_adc(imu_raw_adc);
Lock tx_lock(tx_mav_mutex);
//forward raw ADC
mavlink_msg_huch_imu_raw_adc_encode(system_id(),
component_id,
&tx_mav_msg,
&imu_raw_adc
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//forward MK IMU
//TODO: add compass value and baro
mavlink_huch_mk_imu_t huch_mk_imu;
huch_mk_imu.usec = mkhuch_msg_time;
huch_mk_imu.xacc = (2500*mk_imu->x_acc)/1024; //convert normalized analog to mg
huch_mk_imu.yacc = (2500*mk_imu->y_acc)/1024;
huch_mk_imu.zacc = (2500*mk_imu->z_acc)/1024;
huch_mk_imu.xgyro = (6700*mk_imu->x_adc_gyro)/(3*1024); //convert analog to 0.1 deg./sec.
huch_mk_imu.ygyro = (6700*mk_imu->y_adc_gyro)/(3*1024);
huch_mk_imu.zgyro = (6700*mk_imu->z_adc_gyro)/(3*1024);
DataCenter::set_huch_mk_imu(huch_mk_imu);
mavlink_msg_huch_mk_imu_encode(system_id(),
component_id,
&tx_mav_msg,
&huch_mk_imu
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//forward pressure
mavlink_raw_pressure_t raw_pressure;
raw_pressure.time_usec = mkhuch_msg_time;
raw_pressure.press_abs = mk_imu->press_abs;
raw_pressure.press_diff1 = 0; //press_diff1
raw_pressure.press_diff2 = 0; //press_diff2
raw_pressure.temperature = 0; //temperature
DataCenter::set_raw_pressure(raw_pressure);
mavlink_msg_raw_pressure_encode(system_id(),
component_id,
&tx_mav_msg,
&raw_pressure
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//TODO: forward magneto
break;
}
/* case MKHUCH_MSG_TYPE_PARAM_VALUE: {
const mkhuch_param_t *param = reinterpret_cast<const mkhuch_param_t*>(msg.data);
//set parameter
uint8_t index;
if(param->index >= parameter_count)
index = parameter_count-1;
else
index = param->index;
parameters[index] = param->value;
//ask for next parameter
if(index < parameter_count - 1) {
parameter_request = index + 1;
mk_param_type_t param_type= static_cast<mk_param_type_t>(parameter_request);
send(MKHUCH_MSG_TYPE_PARAM_REQUEST, ¶m_type, sizeof(mk_param_type_t));
parameter_time = message_time;
} else { //got all parameters
parameter_request = 255;
}
//inform others
send_mavlink_param_value( static_cast<mk_param_type_t>(index) );
break;
}*/
/* case MKHUCH_MSG_TYPE_ACTION_ACK: {
Lock tx_lock(tx_mav_mutex);
mavlink_msg_action_ack_pack(owner()->system_id(), component_id, &tx_mav_msg, msg.data[0], msg.data[1]);
send(tx_mav_msg);
break;
}*/
case MKHUCH_MSG_TYPE_STICKS: {
const mkhuch_sticks_t *sticks = reinterpret_cast<const mkhuch_sticks_t*>(msg.data);
mavlink_huch_attitude_control_t attitude_control;
attitude_control.roll = (float)sticks->roll;
attitude_control.pitch = (float)sticks->pitch;
attitude_control.yaw = (float)sticks->yaw;
attitude_control.thrust = (float)sticks->thrust;
attitude_control.target = system_id();
attitude_control.mask = 0;
// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->roll), static_cast<int16_t>(sticks->pitch), Logger::LOGLEVEL_DEBUG);
// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->yaw), static_cast<int16_t>(sticks->thrust), Logger::LOGLEVEL_DEBUG);
// ALERT uint64_t -> uint32_t cast
uint32_t time_ms = get_time_ms();
Lock tx_lock(tx_mav_mutex);
mavlink_msg_huch_attitude_control_encode(
system_id(),
component_id,
&tx_mav_msg,
&attitude_control
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_roll",
// sticks->roll);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_pitch",
// sticks->pitch);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_yaw",
// sticks->yaw);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_thrust",
// sticks->thrust);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MKHUCH_MSG_TYPE_RC_CHANNELS_RAW: {
const mkhuch_rc_channels_raw_t *rc_channels_raw = reinterpret_cast<const mkhuch_rc_channels_raw_t*>(msg.data);
//log("MAVLinkMKHUCHApp got mkhuch_rc_channels_raw msg", static_cast<int16_t>(rc_channels_raw->channel_2), Logger::LOGLEVEL_DEBUG);
Lock tx_lock(tx_mav_mutex);
mavlink_msg_rc_channels_raw_pack(system_id(),
component_id,
&tx_mav_msg,
get_time_ms(),
0, //FIXME
rc_channels_raw->channel_1,
rc_channels_raw->channel_2,
rc_channels_raw->channel_3,
rc_channels_raw->channel_4,
rc_channels_raw->channel_5,
rc_channels_raw->channel_6,
rc_channels_raw->channel_7,
rc_channels_raw->channel_8,
rc_channels_raw->rssi);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MKHUCH_MSG_TYPE_SYSTEM_STATUS: {
/* FIXME
const mkhuch_system_status_t *sys_status = reinterpret_cast<const mkhuch_system_status_t*>(msg.data);
Lock tx_lock(tx_mav_mutex);
mavlink_msg_sys_status_pack(system_id(),
component_id,
&tx_mav_msg,
sys_status->mode,
sys_status->nav_mode,
sys_status->state,
1000, //FIXME: use glibtop to get load of linux system
sys_status->vbat*100, //convert dV to mV
0,//motor block (unsupported)
sys_status->packet_drop);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
*/
break;
}
/* case MKHUCH_MSG_TYPE_BOOT:
//TODO
break;*/
case MKHUCH_MSG_TYPE_ATTITUDE: {
const mkhuch_attitude_t *mkhuch_attitude = reinterpret_cast<const mkhuch_attitude_t*>(msg.data);
mavlink_attitude_t mavlink_attitude;
mavlink_attitude.time_boot_ms = mkhuch_msg_time / 1000;
mavlink_attitude.roll = 0.001745329251994329577*(mkhuch_attitude->roll_angle); //convert cdeg to rad with (pi/180)/10
mavlink_attitude.pitch = 0.001745329251994329577*(mkhuch_attitude->pitch_angle); //convert cdeg to rad with (pi/180)/10
mavlink_attitude.yaw = 0.001745329251994329577*(mkhuch_attitude->yaw_angle); //convert cdeg to rad with (pi/180)/10
//FIXME
mavlink_attitude.rollspeed = 0;
mavlink_attitude.pitchspeed = 0;
mavlink_attitude.yawspeed = 0;
Lock tx_lock(tx_mav_mutex);
mavlink_msg_attitude_encode(system_id(),
component_id,
&tx_mav_msg,
&mavlink_attitude);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
default:
break;
}
}
void DropletCustomOne::DropletMainLoop()
{
switch ( state )
{
case COLLABORATING:
if ( get_32bit_time() - collab_time > COLLABORATE_DURATION )
{
change_state ( LEAVING );
}
break;
case LEAVING:
if ( !is_moving(NULL) )
{
change_state ( SAFE );
}
break;
case SAFE:
if ( check_safe () )
{
change_state ( SEARCHING );
}
// If you start in the red region then try to get out before you die
else
{
random_walk ();
}
break;
case SEARCHING:
random_walk ();
if ( !check_safe() )
{
change_state ( WAITING );
}
break;
case START_DELAY:
{
uint32_t curr_time = get_32bit_time ();
if ( curr_time - start_delay_time > START_DELAY_TIME )
change_state ( SAFE );
}
break;
case WAITING:
if ( get_32bit_time() - heartbeat_time > HEART_RATE )
{
heartbeat_time = get_32bit_time ();
send_heartbeat ();
}
// Checks incoming messages and updates group size.
// There is a chance the state can be changed to COLLABORATING in
// this function if the droplet sees a GO message.
update_group_size ();
if ( get_32bit_time() - voting_time > HEART_RATE &&
state == WAITING )
{
voting_time = get_32bit_time ();
check_votes ();
}
break;
default:
break;
}
}
extern void nmt_slave_send_heartbeat_immediately(co_node *node)
{
send_heartbeat(node);
}
void ControlPack::loop()
{
read_packet();
send_heartbeat();
}
static int in_session(int s, uint8_t** msg, uint32_t modem_heartbeat_interval, uint32_t router_heartbeat_interval)
{
struct timespec last_recv_time = {0};
struct timespec last_sent_time = {0};
struct timespec now_time = {0};
ssize_t received;
struct timeval timeout = {0};
fd_set readfds;
/* Remember when we started */
clock_gettime(CLOCK_MONOTONIC,&now_time);
last_sent_time = last_recv_time = now_time;
/* Make sure we send and check heartbeats promptly - this is a bit hackish */
timeout.tv_sec = (modem_heartbeat_interval < router_heartbeat_interval ? modem_heartbeat_interval : router_heartbeat_interval);
timeout.tv_usec = (timeout.tv_sec % 1000) * 1000;
timeout.tv_sec /= 1000;
/* Loop forever handling messages */
for (;;)
{
/* Wait for a message */
FD_ZERO(&readfds);
FD_SET(s,&readfds);
if (select(s+1,&readfds,NULL,NULL,&timeout) == -1)
{
printf("Failed to wait for message: %s\n",strerror(errno));
return -1;
}
clock_gettime(CLOCK_MONOTONIC,&now_time);
/* Check for our heartbeat interval */
if (interval_compare(&last_sent_time,&now_time,router_heartbeat_interval) > 0)
{
/* Send out a heartbeat if the 'timer' has expired */
send_heartbeat(s,router_heartbeat_interval);
/* Update the last sent time */
last_sent_time = now_time;
}
if (!FD_ISSET(s,&readfds))
{
/* Timeout */
/* Check Modem heartbeat interval, check for 2 missed intervals */
if (interval_compare(&last_recv_time,&now_time,modem_heartbeat_interval * 2) > 0)
{
printf("No heartbeat from modem within %u seconds, terminating session\n",modem_heartbeat_interval * 2);
return send_session_term(s,DLEP_SC_TIMEDOUT,msg,modem_heartbeat_interval);
}
/* Wait again */
continue;
}
/* Receive a message */
received = recv_message(s,msg);
if (received == -1)
{
printf("Failed to receive from TCP socket: %s\n",strerror(errno));
return -1;
}
else if (received == 0)
{
printf("Modem closed TCP session\n");
return -1;
}
else
{
/* Handle the message */
int r = handle_message(s,msg,received,modem_heartbeat_interval);
if (r != 1)
return r;
}
/* Update the last received time */
last_recv_time = now_time;
}
}
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
{
if (telemetry_delayed()) {
return false;
}
// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (!rover.in_mavlink_delay && rover.scheduler.time_available_usec() < 1200) {
gcs().set_out_of_time(true);
return false;
}
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
send_heartbeat();
return true;
case MSG_EXTENDED_STATUS1:
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (initialised) {
CHECK_PAYLOAD_SIZE(SYS_STATUS);
rover.send_extended_status1(chan);
CHECK_PAYLOAD_SIZE(POWER_STATUS);
send_power_status();
}
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
rover.send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
rover.send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (rover.control_mode->is_autopilot_mode()) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan);
}
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
rover.send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
send_radio_in(rover.receiver_rssi);
break;
case MSG_SERVO_OUTPUT_RAW:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_servo_output_raw(false);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
rover.send_vfr_hud(chan);
break;
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu(rover.ins, rover.compass);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_scaled_pressure();
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_sensor_offsets(rover.ins, rover.compass);
break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
rover.send_simstate(chan);
break;
case MSG_RANGEFINDER:
CHECK_PAYLOAD_SIZE(RANGEFINDER);
rover.send_rangefinder(chan);
send_distance_sensor(rover.rangefinder);
send_proximity(rover.g2.proximity);
break;
case MSG_RPM:
CHECK_PAYLOAD_SIZE(RPM);
rover.send_wheel_encoder(chan);
break;
case MSG_MOUNT_STATUS:
#if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
rover.camera_mount.status_msg(chan);
#endif // MOUNT == ENABLED
break;
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
rover.send_fence_status(chan);
break;
case MSG_VIBRATION:
CHECK_PAYLOAD_SIZE(VIBRATION);
send_vibration(rover.ins);
break;
case MSG_BATTERY2:
CHECK_PAYLOAD_SIZE(BATTERY2);
send_battery2(rover.battery);
break;
case MSG_EKF_STATUS_REPORT:
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
rover.ahrs.send_ekf_status_report(chan);
#endif
break;
case MSG_PID_TUNING:
CHECK_PAYLOAD_SIZE(PID_TUNING);
rover.send_pid_tuning(chan);
break;
case MSG_BATTERY_STATUS:
send_battery_status(rover.battery);
break;
default:
return GCS_MAVLINK::try_send_message(id);
}
return true;
}
void CAgentThread::run()
{
AGENT_LOG_INFO("Agent thread ID is %u.\n", get_thread_id());
while (true)
{
try
{
// 必须先建立连接
while (!is_stop()
&& !_connector.is_connect_established())
{
if (connect_center())
break;
}
if (is_stop())
{
AGENT_LOG_INFO("Thread[%u] is tell to stop.\n", get_thread_id());
break;
}
int num = _epoller.timed_wait(_connector.get_connect_timeout_milliseconds());
if (0 == num)
{
// timeout to send heartbeat
AGENT_LOG_DEBUG("Agent timeout to send heartbeat.\n");
send_heartbeat();
}
else
{
for (int i=0; i<num; ++i)
{
uint32_t events = _epoller.get_events(i);
net::CEpollable* epollable = _epoller.get(i);
net::epoll_event_t ee = epollable->handle_epoll_event(NULL, events, NULL);
switch (ee)
{
case net::epoll_read:
_epoller.set_events(epollable, EPOLLIN);
break;
case net::epoll_write:
_epoller.set_events(epollable, EPOLLOUT);
break;
case net::epoll_read_write:
_epoller.set_events(epollable, EPOLLIN | EPOLLOUT);
break;
case net::epoll_remove:
_epoller.del_events(epollable);
break;
case net::epoll_close:
_epoller.del_events(epollable);
epollable->close();
break;
default:
break;
}
}
}
}
catch (sys::CSyscallException& ex)
{
AGENT_LOG_ERROR("Network exception: %s.\n", ex.to_string().c_str());
}
}
_epoller.destroy();
AGENT_LOG_INFO("Agent thread[%u] exited.\n", get_thread_id());
}
