void communication_send_attitude_position(uint64_t loop_start_time) { // ATTITUDE at 20 Hz if (global_data.param[PARAM_SEND_SLOT_ATTITUDE] == 1) { // Send attitude over both UART ports mavlink_msg_attitude_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_offset() + loop_start_time, global_data.attitude.x, global_data.attitude.y, global_data.attitude.z, global_data.gyros_si.x, global_data.gyros_si.y, global_data.gyros_si.z); /* mavlink_msg_attitude_send(MAVLINK_COMM_1, sys_time_clock_get_unix_offset() + loop_start_time, global_data.attitude.x, global_data.attitude.y, global_data.attitude.z, global_data.gyros_si.x, global_data.gyros_si.y, global_data.gyros_si.z);*/ } if (global_data.param[PARAM_SEND_SLOT_DEBUG_5] == 1) { // Send current position and speed mavlink_msg_local_position_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_offset() + loop_start_time, global_data.position.x, global_data.position.y, global_data.position.z, global_data.velocity.x, global_data.velocity.y, global_data.velocity.z); } }
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); } }
void GCS_MAVLINK_Plane::send_attitude() const { const AP_AHRS &ahrs = AP::ahrs(); float r = ahrs.roll; float p = ahrs.pitch - radians(plane.g.pitch_trim_cd*0.01f); float y = ahrs.yaw; if (plane.quadplane.in_vtol_mode()) { r = plane.quadplane.ahrs_view->roll; p = plane.quadplane.ahrs_view->pitch; y = plane.quadplane.ahrs_view->yaw; } const Vector3f &omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, millis(), r, p, y, omega.x, omega.y, omega.z); }
void send(const hrt_abstime t) { if (att_sub->update(t)) { mavlink_msg_attitude_send(_channel, att->timestamp / 1000, att->roll, att->pitch, att->yaw, att->rollspeed, att->pitchspeed, att->yawspeed); } }
void send(const hrt_abstime t) { struct vehicle_attitude_s att; if (att_sub->update(&att_time, &att)) { mavlink_msg_attitude_send(_channel, att.timestamp / 1000, att.roll, att.pitch, att.yaw, att.rollspeed, att.pitchspeed, att.yawspeed); } }
static void mavlink_send_attitude(void) { mavlink_msg_attitude_send(MAVLINK_COMM_0, millis(), _current_state.phi, _current_state.theta, _current_state.psi, _current_state.p, _current_state.q, _current_state.r); }
/** * 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(); }
static NOINLINE void send_attitude(mavlink_channel_t chan) { mavlink_msg_attitude_send( chan, ++buf[1],//millis(), ++buf[2],//ahrs.roll, ++buf[3],//ahrs.pitch, ++buf[4],//ahrs.yaw, ++buf[5],//omega.x, ++buf[6],//omega.y, ++buf[7]);//omega.z); }
/** * 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(); }
void Tracker::send_attitude(mavlink_channel_t chan) { Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, hal.scheduler->millis(), ahrs.roll, ahrs.pitch, ahrs.yaw, omega.x, omega.y, omega.z); }
void Rover::send_attitude(mavlink_channel_t chan) { Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, millis(), ahrs.roll, ahrs.pitch, ahrs.yaw, omega.x, omega.y, omega.z); }
void l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); if (gcs_link) /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, att.roll, att.pitch, att.yaw, att.rollspeed, att.pitchspeed, att.yawspeed); attitude_counter++; }
void sci_mavlinkHilState(scicos_block *block, scicos::enumScicosFlags flag) { // data double * u=GetRealInPortPtrs(block,1); double * y=GetRealOutPortPtrs(block,1); int * ipar=block->ipar; static char * device; static int baudRate; static char ** stringArray; static int * intArray; static int count = 0; static uint16_t packet_drops = 0; //handle flags if (flag==scicos::initialize || flag==scicos::reinitialize) { if (mavlink_comm_0_port == NULL) { getIpars(1,1,ipar,&stringArray,&intArray); device = stringArray[0]; baudRate = intArray[0]; try { mavlink_comm_0_port = new BufferedAsyncSerial(device,baudRate); } catch(const boost::system::system_error & e) { Coserror((char *)e.what()); } } } else if (flag==scicos::terminate) { if (mavlink_comm_0_port) { delete mavlink_comm_0_port; mavlink_comm_0_port = NULL; } } else if (flag==scicos::updateState) { } else if (flag==scicos::computeDeriv) { } else if (flag==scicos::computeOutput) { // channel mavlink_channel_t chan = MAVLINK_COMM_0; // loop rates // TODO: clean this up to use scicos events w/ timers static int attitudeRate = 50; static int positionRate = 10; static int airspeedRate = 1; // initial times double scicosTime = get_scicos_time(); static double attitudeTimeStamp = scicosTime; static double positionTimeStamp = scicosTime; static double airspeedTimeStamp = scicosTime; // send airspeed message if (scicosTime - airspeedTimeStamp > 1.0/airspeedRate) { airspeedTimeStamp = scicosTime; // airspeed (true velocity m/s) float Vt = u[0]; //double rawPress = 1; //double airspeed = 1; //mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0); } else if (scicosTime - airspeedTimeStamp < 0) airspeedTimeStamp = scicosTime; // send attitude message if (scicosTime - attitudeTimeStamp > 1.0/attitudeRate) { attitudeTimeStamp = scicosTime; // attitude states (rad) float roll = u[1]; float pitch = u[2]; float yaw = u[3]; // body rates float rollRate = u[4]; float pitchRate = u[5]; float yawRate = u[6]; mavlink_msg_attitude_send(chan,attitudeTimeStamp,roll,pitch,yaw, rollRate,pitchRate,yawRate); } else if (scicosTime - attitudeTimeStamp < 0) attitudeTimeStamp = scicosTime; // send gps mesage if (scicosTime - positionTimeStamp > 1.0/positionRate) { positionTimeStamp = scicosTime; // gps double cog = u[7]; double sog = u[8]; double lat = u[9]*rad2deg; double lon = u[10]*rad2deg; double alt = u[11]; //double rawPress = 1; //double airspeed = 1; mavlink_msg_gps_raw_send(chan,positionTimeStamp,1,lat,lon,alt,2,10,sog,cog); //mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0); } else if (scicosTime - positionTimeStamp < 0) positionTimeStamp = scicosTime; // receive messages mavlink_message_t msg; mavlink_status_t status; while(comm_get_available(MAVLINK_COMM_0)) { uint8_t c = comm_receive_ch(MAVLINK_COMM_0); // try to get new message if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { //std::cout << "receiving messages" << std::endl; mavlink_rc_channels_scaled_t rc_channels; mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels); y[0] = rc_channels.chan1_scaled/10000.0f; y[1] = rc_channels.chan2_scaled/10000.0f; y[2] = rc_channels.chan3_scaled/10000.0f; y[3] = rc_channels.chan4_scaled/10000.0f; y[4] = rc_channels.chan5_scaled/10000.0f; y[5] = rc_channels.chan6_scaled/10000.0f; y[6] = rc_channels.chan7_scaled/10000.0f; y[7] = rc_channels.chan8_scaled/10000.0f; break; } } // update packet drop counter packet_drops += status.packet_rx_drop_count; } } } }
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
static void stabilizerTask(void* param) { uint32_t lastWakeTime; //uint32_t tempTime; uint16_t heartbCounter = 0; uint16_t attitudeCounter = 0; uint16_t altHoldCounter = 0; //uint32_t data[6]; //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); for( ; ;) { //tempTime = lastWakeTime; vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz heartbCounter ++; /* if (lastWakeTime < tempTime) { tempTime = (0 - tempTime) + lastWakeTime; } else { tempTime = lastWakeTime - tempTime; } */ while (heartbCounter >= HEART_UPDATE_RATE_DIVIDER) { // 1Hz MAVLINK(mavlink_msg_heartbeat_send(MAVLINK_COMM_0, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);) heartbCounter = 0; } imuRead(&gyro, &acc, &mag); if (imu6IsCalibrated()) { // 250HZ if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { MahonyAHRSupdateIMU(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z); //filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z); //filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); radRollActual = eulerRollActual * M_PI / 180.0f; radPitchActual = eulerPitchActual * M_PI / 180.0f; radYawActual = eulerYawActual * M_PI / 180.0f; //float yh, xh; #define yh (mag.y * cos(radRollActual) - mag.z * sin(radRollActual)) #define xh (mag.x*cos(radPitchActual) + mag.y*sin(radRollActual)*sin(radPitchActual) + mag.z * cos(radRollActual)*sin(radPitchActual)) radYawActual = atan2(-yh,xh); MAVLINK(mavlink_msg_attitude_send(MAVLINK_COMM_0, lastWakeTime, \ radRollActual, radPitchActual, radYawActual, \ gyro.x, gyro.y, gyro.z);) attitudeCounter = 0; }