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; } } } }