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 *sensors_raw_receiveloop(void * arg) //runs as a pthread and listens messages from raw sensor
{
uint16_t counter = 0;
uint64_t timestamp;
while(1)
{
if(0 == global_data_wait(&global_data_sensors_raw.access_conf)) //only send if pthread_cond_timedwait received a con signal
{
// TODO: send data to mavlink
// if(counter%100 == 0)
// {
//mavlink_msg_statustext_send(chan,0,"sensor information incoming");
timestamp = global_data_get_timestamp_useconds();
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, timestamp, global_data_sensors_raw.accelerometer_raw[0], global_data_sensors_raw.accelerometer_raw[1], global_data_sensors_raw.accelerometer_raw[2], global_data_sensors_raw.gyro_raw[0], global_data_sensors_raw.gyro_raw[1], global_data_sensors_raw.gyro_raw[2], global_data_sensors_raw.magnetometer_raw[0], global_data_sensors_raw.magnetometer_raw[1], global_data_sensors_raw.magnetometer_raw[2]);
// }
}
// else
// {
// mavlink_msg_statustext_send(chan,0,"timeout");
// }
global_data_unlock(&global_data_sensors_raw.access_conf);
counter++;
}
}
void communication_send_raw_data(uint64_t loop_start_time)
{
if (global_data.param[PARAM_SEND_SLOT_RAW_IMU] == 1)
{
mavlink_msg_raw_imu_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_unix_offset() + loop_start_time,
global_data.accel_raw.x, global_data.accel_raw.y,
global_data.accel_raw.z, global_data.gyros_raw.x,
global_data.gyros_raw.y, global_data.gyros_raw.z,
global_data.magnet_raw.x,
global_data.magnet_raw.y,
global_data.magnet_raw.z);
}
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
const Vector3f &mag = compass.get_field(0);
mavlink_msg_raw_imu_send(
chan,
hal.scheduler->micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
#if INS_MAX_INSTANCES > 1
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &mag2 = compass.get_field(1);
mavlink_msg_scaled_imu2_send(
chan,
hal.scheduler->millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag2.x,
mag2.y,
mag2.z);
#endif
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
Vector3f mag;
if (compass.get_count() >= 1) {
mag = compass.get_field(0);
} else {
mag.zero();
}
mavlink_msg_raw_imu_send(
chan,
AP_HAL::micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
if (compass.get_count() >= 2) {
mag = compass.get_field(1);
} else {
mag.zero();
}
mavlink_msg_scaled_imu2_send(
chan,
AP_HAL::millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 2 &&
ins.get_accel_count() <= 2 &&
compass.get_count() <= 2) {
return;
}
const Vector3f &accel3 = ins.get_accel(2);
const Vector3f &gyro3 = ins.get_gyro(2);
if (compass.get_count() >= 3) {
mag = compass.get_field(2);
} else {
mag.zero();
}
mavlink_msg_scaled_imu3_send(
chan,
AP_HAL::millis(),
accel3.x * 1000.0f / GRAVITY_MSS,
accel3.y * 1000.0f / GRAVITY_MSS,
accel3.z * 1000.0f / GRAVITY_MSS,
gyro3.x * 1000.0f,
gyro3.y * 1000.0f,
gyro3.z * 1000.0f,
mag.x,
mag.y,
mag.z);
}
