void send_system_state(void)
{
// Send heartbeat to announce presence of this system
// Send over both communication links
mavlink_msg_heartbeat_send(MAVLINK_COMM_1,
global_data.param[PARAM_SYSTEM_TYPE], MAV_AUTOPILOT_PIXHAWK);
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
global_data.param[PARAM_SYSTEM_TYPE], MAV_AUTOPILOT_PIXHAWK);
// Send system status over both links
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
// Send auxiliary status over both links
mavlink_msg_aux_status_send(MAVLINK_COMM_1, global_data.cpu_usage,
global_data.i2c0_err_count, global_data.i2c1_err_count,
global_data.spi_err_count, global_data.spi_err_count,
communication_get_uart_drop_rate());
mavlink_msg_aux_status_send(MAVLINK_COMM_0, global_data.cpu_usage,
global_data.i2c0_err_count, global_data.i2c1_err_count,
global_data.spi_err_count, global_data.spi_err_count,
communication_get_uart_drop_rate());
// mavlink_msg_raw_aux_send(MAVLINK_COMM_0, 0, 0, 0, 0,
// battery_get_value(), global_data.temperature,
// global_data.pressure_raw);
}
bool calibration_enter(void)
{
// If not flying
if (!sys_state_is_flying())
{
calibration_prev_state = sys_get_state();
calibration_prev_mode = sys_get_mode();
// Lock vehicle during calibration
sys_set_mode((uint8_t)MAV_MODE_LOCKED);
sys_set_state((uint8_t)MAV_STATE_CALIBRATING);
debug_message_buffer("Starting calibration.");
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
debug_message_send_one();
debug_message_send_one();
return true;
}
else
{
//Can't calibrate during flight
debug_message_buffer("Can't calibrate during flight!!!");
return false;
}
}
void Rover::send_extended_status1(mavlink_channel_t chan)
{
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
update_sensor_status_flags();
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
static_cast<uint16_t>(scheduler.load_average() * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
}
void PROTOCOL_Task( void *pvParameters )
{
mavlink_msg_heartbeat_send(0, global_data.param[PARAM_SYSTEM_TYPE], 0);
mavlink_msg_boot_send(0, global_data.param[PARAM_SW_VERSION]);
//mavlink_msg_statustext_send(0, 1, "FLYless");
while(1)
{
vTaskDelay( 100 / ONE_MS);
ADXL_Convert_to_G(&global_data.acc_raw,&global_data.acc_g);
mavlink_msg_heartbeat_send(0, global_data.param[PARAM_SYSTEM_TYPE], 0);
mavlink_msg_sys_status_send(0, global_data.mode, global_data.nav, global_data.state, 3000, 0, 0);
mavlink_msg_attitude_send(0, 100, global_data.attitude.x,global_data.attitude.y, global_data.attitude.z, global_data.gyro_rad.x, global_data.gyro_rad.y, global_data.gyro_rad.z);
mavlink_msg_raw_imu_send(0, 100, global_data.acc_g.x, global_data.acc_g.y, global_data.acc_g.z, global_data.gyro_raw.x, global_data.gyro_raw.y, global_data.gyro_raw.z, global_data.magnet_raw.x, global_data.magnet_raw.y, global_data.magnet_raw.z);
handle_mav_link_mess();
/* should not be here */
int8_t x1,x2,x3,x4;
x1 = (int8_t)global_data.param[8];
x2 = (int8_t)global_data.param[9];
x3 = (int8_t)global_data.param[10];
x4 = (int8_t)global_data.param[11];
SERVO_SetValue(x1,x2,x3,x4);
}
}
static void *heartbeatloop(void * arg)
{
/* initialize waypoint manager */
mavlink_wpm_init(&wpm);
/* initialize parameter storage */
mavlink_pm_reset_params(&pm);
int lowspeed_counter = 0;
int result_sys_status_trylock;
while(1) {
// sleep
usleep(50000);
// 1 Hz
if (lowspeed_counter == 10)
{
/* Send heartbeat */
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_GENERIC,MAV_MODE_PREFLIGHT,custom_mode,MAV_STATE_UNINIT);
/* Send status */
result_sys_status_trylock = global_data_trylock(&global_data_sys_status.access_conf);
if(0 == result_sys_status_trylock) mavlink_msg_sys_status_send(chan, global_data_sys_status.onboard_control_sensors_present, global_data_sys_status.onboard_control_sensors_enabled, global_data_sys_status.onboard_control_sensors_health, global_data_sys_status.load, global_data_sys_status.voltage_battery, global_data_sys_status.current_battery, global_data_sys_status.battery_remaining, global_data_sys_status.drop_rate_comm, global_data_sys_status.errors_comm, global_data_sys_status.errors_count1, global_data_sys_status.errors_count2, global_data_sys_status.errors_count3, global_data_sys_status.errors_count4);
global_data_unlock(&global_data_sys_status.access_conf);
if (!(mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
// System is not armed, blink at 1 Hz
led_toggle(LED_BLUE);
}
lowspeed_counter = 0;
}
lowspeed_counter++;
// Send one parameter
mavlink_pm_queued_send();
if (mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// System is armed, blink at 10 Hz
led_toggle(LED_BLUE);
}
usleep(50000);
}
}
/**
* Send the system status
*/
static inline void mavlink_send_sys_status(void)
{
mavlink_msg_sys_status_send(MAVLINK_COMM_0,
UAV_SENSORS, // On-board sensors: present (bitmap)
UAV_SENSORS, // On-board sensors: active (bitmap)
UAV_SENSORS, // On-board sensors: state (bitmap)
-1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
100 * electrical.vsupply, // Battery voltage (milivolts)
electrical.current / 10, // Battery current (10x miliampere)
-1, // Battery remaining (0-100 in %)
0, // Communication packet drops (0=0% to 10000=100%)
0, // Communication error(per packet) (0=0% to 10000=100%)
0, // Autopilot specific error 1
0, // Autopilot specific error 2
0, // Autopilot specific error 3
0); // Autopilot specific error 4
MAVLinkSendMessage();
}
void send(const hrt_abstime t)
{
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 100.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
/**
* Send the system status
*/
static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
{
/// TODO: FIXME
#define UAV_SENSORS (MAV_SYS_STATUS_SENSOR_3D_GYRO|MAV_SYS_STATUS_SENSOR_3D_ACCEL|MAV_SYS_STATUS_SENSOR_3D_MAG|MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)
mavlink_msg_sys_status_send(MAVLINK_COMM_0,
UAV_SENSORS, // On-board sensors: present (bitmap)
UAV_SENSORS, // On-board sensors: active (bitmap)
UAV_SENSORS, // On-board sensors: state (bitmap)
-1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
100 * electrical.vsupply, // Battery voltage (milivolts)
electrical.current / 10, // Battery current (10x miliampere)
-1, // Battery remaining (0-100 in %)
0, // Communication packet drops (0=0% to 10000=100%)
0, // Communication error(per packet) (0=0% to 10000=100%)
0, // Autopilot specific error 1
0, // Autopilot specific error 2
0, // Autopilot specific error 3
0); // Autopilot specific error 4
MAVLinkSendMessage();
}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
if (status_sub->update(&status)) {
mavlink_msg_sys_status_send(_channel,
status.onboard_control_sensors_present,
status.onboard_control_sensors_enabled,
status.onboard_control_sensors_health,
status.load * 1000.0f,
status.battery_voltage * 1000.0f,
status.battery_current * 100.0f,
status.battery_remaining * 100.0f,
status.drop_rate_comm,
status.errors_comm,
status.errors_count1,
status.errors_count2,
status.errors_count3,
status.errors_count4);
}
}
//@{
void handle_mavlink_message(mavlink_channel_t chan,
mavlink_message_t* msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint32_t len;
switch (chan)
{
case MAVLINK_COMM_0:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE)
{
// Copy to COMM 1
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart1_transmit(buf[i]);
}
}
}
break;
case MAVLINK_COMM_1:
{
if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE)
{
// Copy to COMM 0
len = mavlink_msg_to_send_buffer(buf, msg);
for (int i = 0; i < len; i++)
{
uart0_transmit(buf[i]);
}
break;
}
}
default:
break;
}
switch (msg->msgid)
{
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(msg, &mode);
// Check if this system should change the mode
if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID])
{
sys_set_mode(mode.mode);
// Emit current mode
mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
global_data.motor_block, communication_get_uart_drop_rate());
}
}
break;
case MAVLINK_MSG_ID_ACTION:
{
execute_action(mavlink_msg_action_get_action(msg));
//Forwart actions from Xbee to Onboard Computer and vice versa
if (chan == MAVLINK_COMM_1)
{
mavlink_send_uart(MAVLINK_COMM_0, msg);
}
else if (chan == MAVLINK_COMM_0)
{
mavlink_send_uart(MAVLINK_COMM_1, msg);
}
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
{
if (!sys_time_clock_get_unix_offset())
{
int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec(
msg)) - (int64_t) sys_time_clock_get_time_usec();
sys_time_clock_set_unix_offset(offset);
debug_message_buffer("UNIX offset updated");
}
else
{
// debug_message_buffer("UNIX offset REFUSED");
}
}
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
mavlink_request_data_stream_t stream;
mavlink_msg_request_data_stream_decode(msg, &stream);
switch (stream.req_stream_id)
{
case 0: // UNIMPLEMENTED
break;
case 1: // RAW SENSOR DATA
global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop;
break;
case 2: // EXTENDED SYSTEM STATUS
global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop;
break;
case 3: // REMOTE CONTROL CHANNELS
global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop;
break;
case 4: // RAW CONTROLLER
//global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
//global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop;
global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop;
break;
case 5: // SENSOR FUSION
//LOST IN GROUDNCONTROL
// global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 6: // POSITION
global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
break;
case 7: // EXTRA1
global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop;
break;
case 8: // EXTRA2
global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop;
break;
case 9: // EXTRA3
global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop;
break;
default:
// Do nothing
break;
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
mavlink_param_request_read_t set;
mavlink_msg_param_request_read_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
if (set.param_id[0] == '\0')
{
// Choose parameter based on index
if (set.param_index < ONBOARD_PARAM_COUNT)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[set.param_index],
global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index);
}
}
else
{
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Report back value
mavlink_msg_param_value_send(chan,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// Start sending parameters
m_parameter_i = 0;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
{
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system
== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
&& (uint8_t) set.target_component
== (uint8_t) global_data.param[PARAM_COMPONENT_ID])
{
char* key = (char*) set.param_id;
for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
{
bool match = true;
for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
{
// Compare
if (((char) (global_data.param_name[i][j]))
!= (char) (key[j]))
{
match = false;
}
// End matching if null termination is reached
if (((char) global_data.param_name[i][j]) == '\0')
{
break;
}
}
// Check if matched
if (match)
{
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infy
if (global_data.param[i] != set.param_value
&& !isnan(set.param_value)
&& !isinf(set.param_value))
{
global_data.param[i] = set.param_value;
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
mavlink_msg_param_value_send(MAVLINK_COMM_1,
(int8_t*) global_data.param_name[i],
global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
debug_message_buffer_sprintf("Parameter received param id=%i",i);
}
}
}
}
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET:
{
mavlink_position_control_setpoint_set_t pos;
mavlink_msg_position_control_setpoint_set_decode(msg, &pos);
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
// global_data.position_setpoint.x = pos.x;
// global_data.position_setpoint.y = pos.y;
// global_data.position_setpoint.z = pos.z;
debug_message_buffer("Position setpoint updated. OLD?\n");
}
else
{
debug_message_buffer(
"Position setpoint recieved but not updated. OLD?\n");
}
// Send back a message confirming the new position
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id,
pos.x, pos.y, pos.z, pos.yaw);
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET:
{
mavlink_position_control_offset_set_t set;
mavlink_msg_position_control_offset_set_decode(msg, &set);
//global_data.attitude_setpoint_pos_body_offset.z = set.yaw;
//Ball Tracking
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1)
{
global_data.param[PARAM_POSITION_SETPOINT_YAW]
= global_data.attitude.z + set.yaw;
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw);
}
}
break;
case MAVLINK_MSG_ID_SET_CAM_SHUTTER:
{
// Decode the desired shutter
mavlink_set_cam_shutter_t cam;
mavlink_msg_set_cam_shutter_decode(msg, &cam);
shutter_set(cam.interval, cam.exposure);
debug_message_buffer_sprintf("set_cam_shutter. interval %i",
cam.interval);
}
break;
case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL:
{
uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg);
shutter_control(enable);
if (enable)
{
debug_message_buffer("CAM: Enabling hardware trigger");
}
else
{
debug_message_buffer("CAM: Disabling hardware trigger");
}
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(msg, &pos);
vision_buffer_handle_data(&pos);
// Update validity time is done in vision buffer
}
break;
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
{
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
global_data.vicon_data.x = pos.x;
global_data.vicon_data.y = pos.y;
global_data.vicon_data.z = pos.z;
global_data.state.vicon_new_data=1;
// Update validity time
global_data.vicon_last_valid = sys_time_clock_get_time_usec();
global_data.state.vicon_ok=1;
// //Set data from Vicon into vision filter
// global_data.vision_data.ang.x = pos.roll;
// global_data.vision_data.ang.y = pos.pitch;
// global_data.vision_data.ang.z = pos.yaw;
//
// global_data.vision_data.pos.x = pos.x;
// global_data.vision_data.pos.y = pos.y;
// global_data.vision_data.pos.z = pos.z;
//
// global_data.vision_data.new_data = 1;
if (!global_data.state.vision_ok)
{
//Correct YAW
global_data.attitude.z = pos.yaw;
//If yaw goes to infy (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z
< -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer(
"vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
}
//send the vicon message to UART0 with new timestamp
mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
}
break;
case MAVLINK_MSG_ID_PING:
{
mavlink_ping_t ping;
mavlink_msg_ping_decode(msg, &ping);
if (ping.target_system == 0 && ping.target_component == 0)
{
// Respond to ping
uint64_t r_timestamp = sys_time_clock_get_unix_time();
mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp);
}
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{
mavlink_local_position_setpoint_set_t sp;
mavlink_msg_local_position_setpoint_set_decode(msg, &sp);
if (sp.target_system == global_data.param[PARAM_SYSTEM_ID])
{
if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
{
if (sp.x >= global_data.position_setpoint_min.x && sp.y
>= global_data.position_setpoint_min.y && sp.z
>= global_data.position_setpoint_min.z && sp.x
<= global_data.position_setpoint_max.x && sp.y
<= global_data.position_setpoint_max.y && sp.z
<= global_data.position_setpoint_max.z)
{
debug_message_buffer("Setpoint accepted and set.");
global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x;
global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y;
global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z;
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0)
{
// Only update yaw if we are not tracking ball.
global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw;
}
//check if we want to start or land
if (global_data.state.status == MAV_STATE_ACTIVE
|| global_data.state.status == MAV_STATE_CRITICAL)
{
if (sp.z > -0.1)
{
if (!(global_data.state.fly == FLY_GROUNDED
|| global_data.state.fly == FLY_SINKING
|| global_data.state.fly
== FLY_WAIT_LANDING
|| global_data.state.fly == FLY_LANDING
|| global_data.state.fly == FLY_RAMP_DOWN))
{
//if setpoint is lower that ground iate landing
global_data.state.fly = FLY_SINKING;
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
debug_message_buffer(
"Sinking for LANDING. (z-sp lower than 10cm)");
}
else if (!(global_data.state.fly == FLY_GROUNDED))
{
global_data.param[PARAM_POSITION_SETPOINT_Z]
= -0.2;//with lowpass
}
}
else if (global_data.state.fly == FLY_GROUNDED && sp.z
< -0.50)
{
//start if we were grounded and get a sp over 0.5m
global_data.state.fly = FLY_WAIT_MOTORS;
debug_message_buffer(
"STARTING wait motors. (z-sp higher than 50cm)");
//set point changed with lowpass, after 5s it will be ok.
}
}
//SINK TO 0.7m if we are critical or emergency
if (global_data.state.status == MAV_STATE_EMERGENCY
|| global_data.state.status == MAV_STATE_CRITICAL)
{
global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass
}
}
else
{
debug_message_buffer("Setpoint refused. Out of range.");
}
}
else
{
debug_message_buffer("Setpoint refused. Param setpoint accept=0.");
}
}
}
break;
default:
break;
}
}
void Rover::send_extended_status1(mavlink_channel_t chan)
{
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
switch (control_mode) {
case MANUAL:
case HOLD:
break;
case LEARNING:
case STEERING:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case AUTO:
case RTL:
case GUIDED:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
break;
case INITIALISING:
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
// default to all healthy except compass and gps which we set individually
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
if (sonar.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.sonar_trigger_cm > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (sonar.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
if (AP_Notify::flags.initialising) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average(20000) * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
}
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
struct vehicle_status_s v_status;
struct actuator_armed_s armed;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
case 'e':
mavlink_link_termination_allowed = true;
break;
case 'o':
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
break;
default:
usage();
}
}
struct termios uart_config_original;
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
err(1, "could not open %s", device_name);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
while (!thread_should_exit) {
/* 1 Hz */
if (lowspeed_counter == 10) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
lowspeed_counter++;
/* sleep 1000 ms */
usleep(1000000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
//_board->debug->printf_P(PSTR("send message\n"));
// if number of channels exceeded return
if (_channel == MAVLINK_COMM_3)
return;
uint64_t timeStamp = micros();
switch (id) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_send(_channel, _board->getVehicle(),
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_send(_channel, timeStamp,
_navigator->getRoll(), _navigator->getPitch(),
_navigator->getYaw(), _navigator->getRollRate(),
_navigator->getPitchRate(), _navigator->getYawRate());
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION: {
mavlink_msg_global_position_send(_channel, timeStamp,
_navigator->getLat() * rad2Deg,
_navigator->getLon() * rad2Deg, _navigator->getAlt(),
_navigator->getVN(), _navigator->getVE(),
_navigator->getVD());
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION: {
mavlink_msg_local_position_send(_channel, timeStamp,
_navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
_navigator->getVN(), _navigator->getVE(), _navigator->getVD());
break;
}
case MAVLINK_MSG_ID_GPS_RAW: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
_board->gps->latitude/1.0e7,
_board->gps->longitude/1.0e7, _board->gps->altitude/100.0, 0, 0,
_board->gps->ground_speed/100.0,
_board->gps->ground_course/10.0);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
_board->gps->latitude,
_board->gps->longitude, _board->gps->altitude*10.0, 0, 0,
_board->gps->ground_speed/100.0,
_board->gps->ground_course/10.0);
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
int16_t xmag, ymag, zmag;
xmag = ymag = zmag = 0;
if (_board->compass) {
// XXX THIS IS NOT SCALED
xmag = _board->compass->mag_x;
ymag = _board->compass->mag_y;
zmag = _board->compass->mag_z;
}
mavlink_msg_scaled_imu_send(_channel, timeStamp,
_navigator->getXAccel()*1e3,
_navigator->getYAccel()*1e3,
_navigator->getZAccel()*1e3,
_navigator->getRollRate()*1e3,
_navigator->getPitchRate()*1e3,
_navigator->getYawRate()*1e3,
xmag, ymag, zmag);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
int16_t ch[8];
for (int i = 0; i < 8; i++)
ch[i] = 0;
for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) {
ch[i] = 10000 * _board->rc[i]->getPosition();
//_board->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
}
mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
int16_t ch[8];
for (int i = 0; i < 8; i++)
ch[i] = 0;
for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++)
ch[i] = _board->rc[i]->getPwm();
mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
uint16_t batteryVoltage = 0; // (milli volts)
uint16_t batteryPercentage = 1000; // times 10
if (_board->batteryMonitor) {
batteryPercentage = _board->batteryMonitor->getPercentage()*10;
batteryVoltage = _board->batteryMonitor->getVoltage()*1000;
}
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
_guide->getMode(), _controller->getState(), _board->load * 10,
batteryVoltage, batteryPercentage, _packetDrops);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
//mavlink_waypoint_ack_t packet;
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
_cmdDestCompId, type);
// turn off waypoint send
_receivingCmds = false;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
break;
}
default: {
char msg[50];
sprintf(msg, "autopilot sending unknown command with id: %d", id);
sendText(SEVERITY_HIGH, msg);
}
} // switch
} // send message
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&lb, 5);
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
case 'e':
mavlink_link_termination_allowed = true;
break;
case 'o':
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
break;
default:
usage();
}
}
struct termios uart_config_original;
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
/* start the ORB receiver */
uorb_receive_thread = uorb_receive_start();
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
/* 0.5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
while (!thread_should_exit) {
/* 1 Hz */
if (lowspeed_counter == 10) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
lowspeed_counter++;
/* sleep quarter the time */
usleep(25000);
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
/* sleep quarter the time */
usleep(25000);
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
/* sleep quarter the time */
usleep(25000);
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
/* sleep 10 ms */
usleep(10000);
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
/* sleep 15 ms */
usleep(15000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
pthread_join(uorb_receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
NOINLINE void Sub::send_sys_status(mavlink_channel_t chan)
{
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (ap.depth_sensor_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
// all present sensors enabled by default except altitude and position control and motors which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_SENSOR_BATTERY);
switch (control_mode) {
case ALT_HOLD:
case AUTO:
case GUIDED:
case CIRCLE:
case SURFACE:
case POSHOLD:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
default:
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// default to all healthy except baro, compass, gps and receiver which we set individually
control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE |
MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
if (sensor_health.depth) { // check the internal barometer only
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.is_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (!battery.healthy() || battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) {
// percent remaining is not necessarily accurate at the moment
//battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
switch (terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
// To-Do: restore unhealthy terrain status reporting once terrain is used in Sub
//control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
//control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
//break;
case AP_Terrain::TerrainStatusOK:
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
control_sensors_health |= MAV_SYS_STATUS_TERRAIN;
break;
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
if (rangefinder_state.enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
if (!ap.initialised || ins.calibrating()) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average() * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
// FlightTelemetryStatsData flightStats;
// GCSTelemetryStatsData gcsTelemetryStatsData;
// int32_t retries;
// int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
}
mavlink_message_t msg;
mavlink_system.sysid = 20;
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.type = MAV_TYPE_FIXED_WING;
uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT;
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
// Setup type and object id fields
uint32_t objId = UAVObjGetID(ev->obj);
// uint64_t timeStamp = 0;
switch(objId) {
case BAROALTITUDE_OBJID:
{
BaroAltitudeGet(&baroAltitude);
pressure.press_abs = baroAltitude.Pressure*10.0f;
pressure.temperature = baroAltitude.Temperature*100.0f;
mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case FLIGHTTELEMETRYSTATS_OBJID:
{
// FlightTelemetryStatsData flightTelemetryStats;
FlightTelemetryStatsGet(&flightStats);
// XXX this is a hack to make it think it got a confirmed
// connection
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
GCSTelemetryStatsGet(&gcsTelemetryStatsData);
gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
//
//
// //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass);
// mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case SYSTEMSTATS_OBJID:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t system_state = MAV_STATE_UNINIT;
uint8_t base_mode = 0;
uint8_t custom_mode = 0;
// Set flight mode
switch (flightStatus.FlightMode)
{
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
default:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
}
// Set arming state
switch (flightStatus.Armed)
{
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
system_state = MAV_STATE_ACTIVE;
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case FLIGHTSTATUS_ARMED_DISARMED:
system_state = MAV_STATE_STANDBY;
base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED;
break;
}
// Set HIL
if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state);
SystemStatsData stats;
SystemStatsGet(&stats);
FlightBatteryStateData flightBatteryData;
FlightBatteryStateGet(&flightBatteryData);
FlightBatterySettingsData flightBatterySettings;
FlightBatterySettingsGet(&flightBatterySettings);
uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f);
int16_t batteryCurrent = -1; // -1: Not present / not estimated
int8_t batteryPercent = -1; // -1: Not present / not estimated
// if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0)
// {
// Factor is zero, sensor is not present
// Estimate remaining capacity based on lipo curve
batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f));
// }
// else
// {
// // Use capacity and current
// batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity);
// batteryCurrent = flightBatteryData.Current*100;
// }
mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0);
// // Copy the message to the send buffer
// uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// // Send buffer
// PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case ATTITUDERAW_OBJID:
{
AttitudeRawGet(&attitudeRaw);
// Copy data
attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X];
attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y];
attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z];
attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X];
attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y];
attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z];
attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X];
attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y];
attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z];
mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
if (hilEnabled)
{
mavlink_hil_controls_t controls;
// Copy data
controls.roll_ailerons = 0.1;
controls.pitch_elevator = 0.1;
controls.yaw_rudder = 0.0;
controls.throttle = 0.8;
mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
}
case ATTITUDEMATRIX_OBJID:
{
AttitudeMatrixGet(&attitudeMatrix);
// Copy data
attitude.roll = attitudeMatrix.Roll;
attitude.pitch = attitudeMatrix.Pitch;
attitude.yaw = attitudeMatrix.Yaw;
attitude.rollspeed = attitudeMatrix.AngularRates[0];
attitude.pitchspeed = attitudeMatrix.AngularRates[1];
attitude.yawspeed = attitudeMatrix.AngularRates[2];
mavlink_msg_attitude_encode(mavlink_system.sysid,
mavlink_system.compid, &msg, &attitude);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
break;
}
case GPSPOSITION_OBJID:
{
GPSPositionGet(&gpsPosition);
gps_raw.time_usec = 0;
gps_raw.lat = gpsPosition.Latitude*10;
gps_raw.lon = gpsPosition.Longitude*10;
gps_raw.alt = gpsPosition.Altitude*10;
gps_raw.eph = gpsPosition.HDOP*100;
gps_raw.epv = gpsPosition.VDOP*100;
gps_raw.cog = gpsPosition.Heading*100;
gps_raw.satellites_visible = gpsPosition.Satellites;
gps_raw.fix_type = gpsPosition.Status;
mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
// mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0);
break;
}
case POSITIONACTUAL_OBJID:
{
PositionActualData pos;
PositionActualGet(&pos);
mavlink_local_position_ned_t m_pos;
m_pos.time_boot_ms = 0;
m_pos.x = pos.North;
m_pos.y = pos.East;
m_pos.z = pos.Down;
m_pos.vx = 0.0f;
m_pos.vy = 0.0f;
m_pos.vz = 0.0f;
mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len);
}
break;
case ACTUATORCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
ActuatorCommandData act;
ActuatorCommandGet(&act);
rc.chan5_scaled = act.Channel[0];
rc.chan6_scaled = act.Channel[1];
rc.chan7_scaled = act.Channel[2];
rc.chan8_scaled = act.Channel[3];
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
case MANUALCONTROLCOMMAND_OBJID:
{
mavlink_rc_channels_scaled_t rc;
float val;
ManualControlCommandRollGet(&val);
rc.chan1_scaled = val*1000;
ManualControlCommandPitchGet(&val);
rc.chan2_scaled = val*1000;
ManualControlCommandYawGet(&val);
rc.chan3_scaled = val*1000;
ManualControlCommandThrottleGet(&val);
rc.chan4_scaled = val*1000;
rc.chan5_scaled = 0;
rc.chan6_scaled = 0;
rc.chan7_scaled = 0;
rc.chan8_scaled = 0;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255;
rc.port = 0;
mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg);
// Send buffer
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len);
break;
}
default:
{
//printf("unknown object: %x\n",(unsigned int)objId);
break;
}
}
}
}
