// ------------------------------------------------------------------------------
// Write Heartbeat message
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
write_heartbeat()
{
// --------------------------------------------------------------------------
// PACK PAYLOAD
// --------------------------------------------------------------------------
// Allocate Space
mavlink_message_t msg;
mavlink_heartbeat_t heartbeat;
struct timespec t_now_rt,t_diff_rt;
uint64_t timestamp = get_time_usec();
// Pack the message
//mavlink_msg_heartbeat_pack(system_id, companion_id, &msg, mav_type, autopilot_id , base_mode, custom_mode, system_status)
heartbeat.type = MAV_TYPE_GCS;
heartbeat.autopilot = autopilot_id;
heartbeat.base_mode = base_mode;
heartbeat.system_status = system_status;
heartbeat.custom_mode = 0;
mavlink_msg_heartbeat_encode(system_id, companion_id, &msg, &heartbeat);
//mavlink_msg_heartbeat_encode(2, 125, &msg, &heartbeat);
// --------------------------------------------------------------------------
// WRITE
// --------------------------------------------------------------------------
clock_gettime(CLOCK_REALTIME,&t_now_rt);
int len = write_message(msg);
timespec_sub(&t_diff_rt,&t_now_rt,&t_wrold_rt);
int64_t diff_rt = t_diff_rt.tv_sec*1e6 + t_diff_rt.tv_nsec/1e3;
//clock_gettime(CLOCK_REALTIME,&t_wrold_rt); // With this command I obtained a DT with the writing time in the computation
t_wrold_rt.tv_nsec = t_now_rt.tv_nsec;
t_wrold_rt.tv_sec = t_now_rt.tv_sec;
//printf("hil_sensors_t DT : %lu us\r",diff_rt);
// check the write
if ( not len > 0 )
fprintf(stderr,"WARNING: could not send HIL_SENSORS \n");
// else
// printf("%lu POSITION_TARGET = [ %f , %f , %f ] \n", write_count, position_target.x, position_target.y, position_target.z);
return;
}
void mavlink_connector::slow_send()
{
mavlink_gps_raw_int_t gps_raw_int_t;
mavlink_battery_status_t battery_status_t;
mavlink_rc_channels_t rc_channels_t;
mavlink_message_t msg;
gps_raw_int_t.lat = (int32_t)(dji_variable::global_position_degree.lati * 1e7) ;
gps_raw_int_t.lon = (int32_t)(dji_variable::global_position_degree.longti * 1e7);
gps_raw_int_t.alt = (int32_t)(dji_variable::global_position_degree.alti * 1000);
mavlink_msg_gps_raw_int_encode(0,200,&msg,&gps_raw_int_t);
send_msg(&msg);
mavlink_msg_heartbeat_encode(0,200,&msg,make_heartbeat());
send_msg(&msg);
// printf("sended heartbeating......\n");
battery_status_t.battery_remaining = dji_variable::battery;
for (int i = 0 ;i< 6;i++)
battery_status_t.voltages[i] = (uint16_t)((float)dji_variable::battery / 100.0f * 22.2 * 1000);
mavlink_msg_battery_status_encode(0,200,&msg,&battery_status_t);
send_msg(&msg);
mavlink_sys_status_t sys_status_t;
memset(&sys_status_t,0, sizeof(sys_status_t));
sys_status_t.battery_remaining = dji_variable::battery;
sys_status_t.voltage_battery = 222 * dji_variable::battery ;
mavlink_msg_sys_status_encode(0,200,&msg,&sys_status_t);
send_msg(&msg);
mavlink_rc_channels_scaled_t rc_channels_scaled_t;
rc_channels_scaled_t.chan1_scaled = dji_variable::rc_channels.roll;
rc_channels_scaled_t.chan2_scaled = dji_variable::rc_channels.pitch;
rc_channels_scaled_t.chan3_scaled = dji_variable::rc_channels.throttle;
rc_channels_scaled_t.chan4_scaled = dji_variable::rc_channels.yaw;
rc_channels_scaled_t.chan5_scaled = dji_variable::rc_channels.mode;
rc_channels_scaled_t.chan6_scaled = dji_variable::rc_channels.gear;
mavlink_msg_rc_channels_scaled_encode(0,200,&msg,&rc_channels_scaled_t);
send_msg(&msg);
}
//called periodically
Void cron_quick_clock(UArg arg){
// flash led
GPIO_toggle(Board_LED_GREEN); // use red led for user inputs
/* MAVLINK HEARTBEAT */
// Initialize the message buffer
static COMM_FRAME frame;
mavlink_heartbeat_t hb=comm_get_mavlink_heartbeat();
// Pack the message
mavlink_msg_heartbeat_encode(mavlink_system.sysid, mavlink_system.compid, &(frame.mavlink_message), &hb);
comm_mavlink_broadcast(&frame); //send heartbeat for all available channel slots
}
void Test::sendHearbeat(bool armed, MAV_STATE state)
{
mavlink_heartbeat_t hearbeat;
hearbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
hearbeat.base_mode = 0;
if (armed)
{
hearbeat.base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
hearbeat.custom_mode = Aircraft::CustomMode_PositionHold;
hearbeat.mavlink_version = 1;
hearbeat.system_status = state;
hearbeat.type = MAV_TYPE_QUADROTOR;
// hearbeat.type = MAV_TYPE_GCS;
mavlink_message_t buf;
mavlink_msg_heartbeat_encode(SYSTEM_ID, COMPONENT_ID, &buf, &hearbeat);
testMessage(buf);
}
/*
send a report to the vehicle control code over MAVLink
*/
void Gimbal::send_report(void)
{
if (!mavlink.connected && mav_socket.connect(target_address, target_port)) {
::printf("Gimbal connected to %s:%u\n", target_address, (unsigned)target_port);
mavlink.connected = true;
}
if (!mavlink.connected) {
return;
}
// check for incoming MAVLink messages
uint8_t buf[100];
ssize_t ret;
while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
for (uint8_t i=0; i<ret; i++) {
mavlink_message_t msg;
mavlink_status_t status;
if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
buf[i],
&msg, &status) == MAVLINK_FRAMING_OK) {
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
if (!seen_heartbeat) {
seen_heartbeat = true;
vehicle_component_id = msg.compid;
vehicle_system_id = msg.sysid;
::printf("Gimbal using srcSystem %u\n", (unsigned)vehicle_system_id);
}
break;
}
case MAVLINK_MSG_ID_GIMBAL_CONTROL: {
mavlink_gimbal_control_t pkt;
mavlink_msg_gimbal_control_decode(&msg, &pkt);
demanded_angular_rate = Vector3f(pkt.demanded_rate_x,
pkt.demanded_rate_y,
pkt.demanded_rate_z);
// no longer supply a bias
supplied_gyro_bias.zero();
seen_gimbal_control = true;
break;
}
}
}
}
}
if (!seen_heartbeat) {
return;
}
uint32_t now = hal.scheduler->millis();
mavlink_message_t msg;
uint16_t len;
if (now - last_heartbeat_ms >= 1000) {
mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_GIMBAL;
heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
heartbeat.base_mode = 0;
heartbeat.system_status = 0;
heartbeat.mavlink_version = 0;
heartbeat.custom_mode = 0;
/*
save and restore sequence number for chan0, as it is used by
generated encode functions
*/
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_heartbeat_encode(vehicle_system_id,
vehicle_component_id,
&msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len);
last_heartbeat_ms = now;
}
/*
send a GIMBAL_REPORT message
*/
uint32_t now_us = hal.scheduler->micros();
if (now_us - last_report_us > reporting_period_ms*1000UL) {
mavlink_gimbal_report_t gimbal_report;
float delta_time = (now_us - last_report_us) * 1.0e-6f;
last_report_us = now_us;
gimbal_report.target_system = vehicle_system_id;
gimbal_report.target_component = vehicle_component_id;
gimbal_report.delta_time = delta_time;
gimbal_report.delta_angle_x = delta_angle.x;
gimbal_report.delta_angle_y = delta_angle.y;
gimbal_report.delta_angle_z = delta_angle.z;
gimbal_report.delta_velocity_x = delta_velocity.x;
gimbal_report.delta_velocity_y = delta_velocity.y;
gimbal_report.delta_velocity_z = delta_velocity.z;
gimbal_report.joint_roll = joint_angles.x;
gimbal_report.joint_el = joint_angles.y;
gimbal_report.joint_az = joint_angles.z;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_gimbal_report_encode(vehicle_system_id,
vehicle_component_id,
&msg, &gimbal_report);
chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len);
delta_velocity.zero();
delta_angle.zero();
}
}
void loop(void)
{
uint16_t err_count = 0;
// incoming heartbeat
mavlink_message_t msg;
mavlink_heartbeat_t heartbeat = {0};
mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("heartbeat should be processed locally\n");
err_count++;
}
// incoming non-targetted message
mavlink_attitude_t attitude = {0};
mavlink_msg_attitude_encode(3, 1, &msg, &attitude);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("attitude should be processed locally\n");
err_count++;
}
// incoming targeted message for someone else
mavlink_param_set_t param_set = {0};
param_set.target_system = mavlink_system.sysid+1;
param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 1 should not be processed locally\n");
err_count++;
}
// incoming targeted message for us
param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 2 should be processed locally\n");
err_count++;
}
// incoming targeted message for our system, but other compid
// should be processed locally
param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 3 should be processed locally\n");
err_count++;
}
// incoming broadcast message should be processed locally
param_set.target_system = 0;
param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) {
hal.console->printf("param set 4 should be processed locally\n");
err_count++;
}
if (err_count == 0) {
hal.console->printf("All OK\n");
}
hal.scheduler->delay(1000);
}
/*
send a report to the vehicle control code over MAVLink
*/
void ADSB::send_report(void)
{
if (AP_HAL::millis() < 10000) {
// simulated aircraft don't appear until 10s after startup. This avoids a windows
// threading issue with non-blocking sockets and the initial wait on uartA
return;
}
if (!mavlink.connected && mav_socket.connect(target_address, target_port)) {
::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port);
mavlink.connected = true;
}
if (!mavlink.connected) {
return;
}
// check for incoming MAVLink messages
uint8_t buf[100];
ssize_t ret;
while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
for (uint8_t i=0; i<ret; i++) {
mavlink_message_t msg;
mavlink_status_t status;
if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
buf[i],
&msg, &status) == MAVLINK_FRAMING_OK) {
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
if (!seen_heartbeat) {
seen_heartbeat = true;
vehicle_component_id = msg.compid;
vehicle_system_id = msg.sysid;
::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id);
}
break;
}
}
}
}
}
if (!seen_heartbeat) {
return;
}
uint32_t now = AP_HAL::millis();
mavlink_message_t msg;
uint16_t len;
if (now - last_heartbeat_ms >= 1000) {
mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_ADSB;
heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
heartbeat.base_mode = 0;
heartbeat.system_status = 0;
heartbeat.mavlink_version = 0;
heartbeat.custom_mode = 0;
/*
save and restore sequence number for chan0, as it is used by
generated encode functions
*/
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_heartbeat_encode(vehicle_system_id,
vehicle_component_id,
&msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len);
last_heartbeat_ms = now;
}
/*
send a ADSB_VEHICLE messages
*/
uint32_t now_us = AP_HAL::micros();
if (now_us - last_report_us >= reporting_period_ms*1000UL) {
for (uint8_t i=0; i<num_vehicles; i++) {
ADSB_Vehicle &vehicle = vehicles[i];
Location loc = home;
location_offset(loc, vehicle.position.x, vehicle.position.y);
// re-init when exceeding radius range
if (get_distance(home, loc) > _sitl->adsb_radius_m) {
vehicle.initialised = false;
}
mavlink_adsb_vehicle_t adsb_vehicle {};
last_report_us = now_us;
adsb_vehicle.ICAO_address = vehicle.ICAO_address;
adsb_vehicle.lat = loc.lat;
adsb_vehicle.lon = loc.lng;
adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
adsb_vehicle.altitude = -vehicle.position.z * 1000;
adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x)));
adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100;
adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100;
memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign));
adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE;
adsb_vehicle.tslc = 1;
adsb_vehicle.flags =
ADSB_FLAGS_VALID_COORDS |
ADSB_FLAGS_VALID_ALTITUDE |
ADSB_FLAGS_VALID_HEADING |
ADSB_FLAGS_VALID_VELOCITY |
ADSB_FLAGS_VALID_CALLSIGN |
ADSB_FLAGS_SIMULATED;
adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id,
MAV_COMP_ID_ADSB,
&msg, &adsb_vehicle);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
}
}
}
// ADSB_transceiever is enabled, send the status report.
if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) {
last_tx_report_ms = now;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
uint8_t saved_flags = chan0_status->flags;
chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
chan0_status->current_tx_seq = mavlink.seq;
const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK};
len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id,
MAV_COMP_ID_ADSB,
&msg, &health_report);
chan0_status->current_tx_seq = saved_seq;
chan0_status->flags = saved_flags;
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
::printf("ADSBsim send tx health packet\n");
}
}
}
// ------------------------------------------------------------------------------
// Top
// ------------------------------------------------------------------------------
int
top (int argc, char **arv)
{
// --------------------------------------------------------------------------
// SETUP!!
// --------------------------------------------------------------------------
// sudo apt-get install socat (only once)
// sudo chown <user> /dev (only run once)
// socat -d -d pty,raw,echo=0,link=/dev/ttyVA00 pty,raw,echo=0,link=/dev/ttyUSB0
// or!!
// ./start_pseudo_mavlink.sh starts everthing
// --------------------------------------------------------------------------
// Start Port
// --------------------------------------------------------------------------
char *uart_name = (char*)"/dev/ttyVA00"; // pseudo teletype
int baudrate = 57600;
Serial_Port serial_port(uart_name,baudrate);
serial_port.start();
// OWNSHIP POSITION AND VELOCITY, LOCAL_NED
mavlink_local_position_ned_t lp;
lp.time_boot_ms = (uint32_t) (get_time_usec()/1000);
lp.x = 0.1;
lp.y = 0.2;
lp.z = 0.3;
lp.vx = 0.4;
lp.vy = 0.5;
lp.vz = 0.0;
mavlink_attitude_t att;
att.roll = 0;
att.pitch = 0;
att.yaw = 0;
att.rollspeed = 0;
att.pitchspeed = 0;
att.yawspeed = 0;
mavlink_heartbeat_t hb;
hb.custom_mode = 0;
hb.type = 0;
hb.autopilot = 0;
hb.base_mode = 0;
hb.system_status = 0;
hb.mavlink_version = 0;
while (1)
{
mavlink_message_t lp_m, att_m, hb_m;
mavlink_msg_local_position_ned_encode(system_id, component_id, &lp_m, &lp);
mavlink_msg_attitude_encode(system_id, component_id, &att_m, &att);
mavlink_msg_heartbeat_encode(system_id, component_id, &hb_m, &hb);
serial_port.write_message(lp_m);
serial_port.write_message(att_m);
serial_port.write_message(hb_m);
usleep(0.5e6); //2Hz
}
return 0;
}
