static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev) { mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetPositionNed_f()->x, stateGetPositionNed_f()->y, stateGetPositionNed_f()->z, stateGetSpeedNed_f()->x, stateGetSpeedNed_f()->y, stateGetSpeedNed_f()->z); MAVLinkSendMessage(); }
static inline void mavlink_send_local_position_ned(void) { mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetPositionNed_f()->x, stateGetPositionNed_f()->y, stateGetPositionNed_f()->z, stateGetSpeedNed_f()->x, stateGetSpeedNed_f()->y, stateGetSpeedNed_f()->z); MAVLinkSendMessage(); }
/** * Send the attitude */ static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev) { mavlink_msg_attitude_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyEulers_f()->phi, // Phi stateGetNedToBodyEulers_f()->theta, // Theta stateGetNedToBodyEulers_f()->psi, // Psi stateGetBodyRates_f()->p, // p stateGetBodyRates_f()->q, // q stateGetBodyRates_f()->r); // r MAVLinkSendMessage(); }
/** * Send the attitude */ static inline void mavlink_send_attitude(void) { mavlink_msg_attitude_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyEulers_f()->phi, // Phi stateGetNedToBodyEulers_f()->theta, // Theta stateGetNedToBodyEulers_f()->psi, // Psi stateGetBodyRates_f()->p, // p stateGetBodyRates_f()->q, // q stateGetBodyRates_f()->r); // r MAVLinkSendMessage(); }
static inline void mavlink_send_attitude_quaternion(void) { mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyQuat_f()->qi, stateGetNedToBodyQuat_f()->qx, stateGetNedToBodyQuat_f()->qy, stateGetNedToBodyQuat_f()->qz, stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); MAVLinkSendMessage(); }
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev) { mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyQuat_f()->qi, stateGetNedToBodyQuat_f()->qx, stateGetNedToBodyQuat_f()->qy, stateGetNedToBodyQuat_f()->qz, stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); MAVLinkSendMessage(); }
/** * Send the parameters */ static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev) { if (mavlink_params_idx >= NB_SETTING) { return; } mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[mavlink_params_idx], settings_get_value(mavlink_params_idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, mavlink_params_idx); MAVLinkSendMessage(); mavlink_params_idx++; }
/** * Send the parameters */ static inline void mavlink_send_params(void) { if (mavlink_params_idx >= NB_SETTING) { return; } mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[mavlink_params_idx], settings_get_value(mavlink_params_idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, mavlink_params_idx); MAVLinkSendMessage(); mavlink_params_idx++; }
/** * 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(); }
static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev) { static uint16_t voltages[10]; // we simply only set one cell for now voltages[0] = electrical.vsupply * 100; /// TODO: check what all these fields are supposed to represent mavlink_msg_battery_status_send(MAVLINK_COMM_0, 0, // id 0, // battery_function 0, // type INT16_MAX, // unknown temperature voltages, // cell voltages electrical.current / 10, electrical.consumed, electrical.energy, // check scaling -1); // remaining percentage not estimated MAVLinkSendMessage(); }
/** * 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(); }
/** * Send Metrics typically displayed on a HUD for fixed wing aircraft. */ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev) { /* Current heading in degrees, in compass units (0..360, 0=north) */ int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); /* Current throttle setting in integer percent, 0 to 100 */ // is a 16bit unsigned int but supposed to be from 0 to 100?? uint16_t throttle; #ifdef COMMAND_THRUST throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100); #elif defined COMMAND_THROTTLE throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100); #endif mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, stateGetAirspeed_f(), stateGetHorizontalSpeedNorm_f(), // groundspeed heading, throttle, stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL stateGetSpeedNed_f()->z); // climb rate MAVLinkSendMessage(); }
static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev) { #if USE_GPS int32_t course = DegOfRad(gps.course) / 1e5; if (course < 0) { course += 36000; } mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, get_sys_time_usec(), gps.fix, gps.lla_pos.lat, gps.lla_pos.lon, gps.lla_pos.alt, gps.pdop, UINT16_MAX, // VDOP gps.gspeed, course, gps.num_sv); MAVLinkSendMessage(); #endif }
static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev) { /// TODO: fill in versions correctly, how should they be encoded? static uint32_t ver = PPRZ_VERSION_INT; static uint64_t sha; get_pprz_git_version((uint8_t *)&sha); mavlink_msg_autopilot_version_send(MAVLINK_COMM_0, 0, //uint64_t capabilities, ver, //uint32_t flight_sw_version, 0, //uint32_t middleware_sw_version, 0, //uint32_t os_sw_version, 0, //uint32_t board_version, 0, //const uint8_t *flight_custom_version, 0, //const uint8_t *middleware_custom_version, 0, //const uint8_t *os_custom_version, 0, //uint16_t vendor_id, 0, //uint16_t product_id, sha //uint64_t uid ); MAVLinkSendMessage(); }
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev) { float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); if (heading < 0.) { heading += 360; } uint16_t compass_heading = heading * 100; int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl; /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs mavlink_msg_global_position_int_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetPositionLla_i()->lat, stateGetPositionLla_i()->lon, stateGetPositionLla_i()->alt, relative_alt, stateGetSpeedNed_f()->x * 100, stateGetSpeedNed_f()->y * 100, stateGetSpeedNed_f()->z * 100, compass_heading); MAVLinkSendMessage(); }
/** * Send a heartbeat */ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev) { uint8_t mav_state = MAV_STATE_CALIBRATING; uint8_t mav_mode = 0; #ifdef AP uint8_t mav_type = MAV_TYPE_FIXED_WING; switch (pprz_mode) { case PPRZ_MODE_MANUAL: mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case PPRZ_MODE_AUTO1: mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case PPRZ_MODE_AUTO2: mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case PPRZ_MODE_HOME: mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED; break; default: break; } #else uint8_t mav_type = MAV_TYPE_QUADROTOR; switch (autopilot_mode) { case AP_MODE_HOME: mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_RC_CLIMB: case AP_MODE_RATE_Z_HOLD: case AP_MODE_RC_DIRECT: mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_ATTITUDE_RC_CLIMB: case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: case AP_MODE_CARE_FREE_DIRECT: mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case AP_MODE_NAV: mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case AP_MODE_GUIDED: mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED; default: break; } #endif if (stateIsAttitudeValid()) { if (kill_throttle) { mav_state = MAV_STATE_STANDBY; } else { mav_state = MAV_STATE_ACTIVE; mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } } mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mav_type, MAV_AUTOPILOT_PPZ, mav_mode, 0, // custom_mode mav_state); MAVLinkSendMessage(); }
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); 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 == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; default: //Do nothing //MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
/** * Send SYSTEM_TIME * - time_unix_usec * - time_boot_ms */ static inline void mavlink_send_system_time(void) { mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec()); MAVLinkSendMessage(); }
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } // Send message only if the param_index was found (Coverity Scan) if (cmd.param_index > -1) { mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); } 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 == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; #ifndef AP /* only for rotorcraft */ case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); // Check if this message is for this system if ((uint8_t) cmd.target_system == AC_ID) { uint8_t result = MAV_RESULT_UNSUPPORTED; switch (cmd.command) { case MAV_CMD_NAV_GUIDED_ENABLE: MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_mode(AP_MODE_GUIDED); if (autopilot_mode == AP_MODE_GUIDED) { result = MAV_RESULT_ACCEPTED; } } else { // turn guided mode off - to what? maybe NAV? or MODE_AUTO2? } break; case MAV_CMD_COMPONENT_ARM_DISARM: /* supposed to use this command to arm or SET_MODE?? */ MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_motors_on(TRUE); if (autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } else { autopilot_set_motors_on(FALSE); if (!autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } break; default: break; } // confirm command with result mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); if (mode.target_system == AC_ID) { MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode); if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { autopilot_set_motors_on(TRUE); } else { autopilot_set_motors_on(FALSE); } if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { autopilot_set_mode(AP_MODE_GUIDED); } else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { autopilot_set_mode(AP_MODE_NAV); } } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { mavlink_set_position_target_local_ned_t target; mavlink_msg_set_position_target_local_ned_decode(msg, &target); // Check if this message is for this system if (target.target_system == AC_ID) { MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask); /* if position and yaw bits are not set to ignored, use only position for now */ if (!(target.type_mask & 0b1110000000100000)) { switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set position target, frame LOCAL_NED\n"); autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_LOCAL_OFFSET_NED: MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n"); autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_BODY_OFFSET_NED: MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n"); autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw); break; default: break; } } else if (!(target.type_mask & 0b0001110000100000)) { /* position is set to ignore, but velocity not */ switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n"); autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw); break; default: break; } } } break; } #endif default: //Do nothing MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
/** * Send SYSTEM_TIME * - time_unix_usec * - time_boot_ms */ static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev) { mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec()); MAVLinkSendMessage(); }