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