mirror of https://github.com/ArduPilot/ardupilot
420 lines
11 KiB
C
420 lines
11 KiB
C
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#ifndef Mavlink_Common_H
|
|
#define Mavlink_Common_H
|
|
|
|
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
|
|
|
byte mavdelay = 0;
|
|
|
|
|
|
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
|
{
|
|
//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC);
|
|
if (sysid != mavlink_system.sysid){
|
|
return 1;
|
|
|
|
// Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem
|
|
// If it is addressed to our system ID we assume it is for us
|
|
//}else if(compid != mavlink_system.compid){
|
|
// gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch"));
|
|
// return 1; // XXX currently not receiving correct compid from gcs
|
|
|
|
}else{
|
|
return 0; // no error
|
|
}
|
|
}
|
|
|
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
|
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
|
|
{
|
|
uint64_t timeStamp = micros();
|
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
|
|
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
|
|
|
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
|
// defer any messages on the telemetry port for 1 second after
|
|
// bootup, to try to prevent bricking of Xbees
|
|
return false;
|
|
}
|
|
|
|
switch(id) {
|
|
|
|
case MSG_HEARTBEAT:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
|
mavlink_msg_heartbeat_send(
|
|
chan,
|
|
mavlink_system.type,
|
|
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
|
break;
|
|
}
|
|
|
|
case MSG_EXTENDED_STATUS1:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
|
|
|
uint8_t mode = MAV_MODE_UNINIT;
|
|
uint8_t nav_mode = MAV_NAV_VECTOR;
|
|
|
|
switch(control_mode) {
|
|
case LOITER:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_HOLD;
|
|
break;
|
|
case AUTO:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_WAYPOINT;
|
|
break;
|
|
case RTL:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_RETURNING;
|
|
break;
|
|
case GUIDED:
|
|
mode = MAV_MODE_GUIDED;
|
|
break;
|
|
default:
|
|
mode = control_mode + 100;
|
|
|
|
}
|
|
|
|
uint8_t status = MAV_STATE_ACTIVE;
|
|
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
|
|
|
mavlink_msg_sys_status_send(
|
|
chan,
|
|
mode,
|
|
nav_mode,
|
|
status,
|
|
0,
|
|
battery_voltage * 1000,
|
|
battery_remaining,
|
|
packet_drops);
|
|
break;
|
|
}
|
|
|
|
case MSG_EXTENDED_STATUS2:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(MEMINFO);
|
|
extern unsigned __brkval;
|
|
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
|
break;
|
|
}
|
|
|
|
case MSG_ATTITUDE:
|
|
{
|
|
//Vector3f omega = dcm.get_gyro();
|
|
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
|
mavlink_msg_attitude_send(
|
|
chan,
|
|
timeStamp,
|
|
dcm.roll,
|
|
dcm.pitch,
|
|
dcm.yaw,
|
|
omega.x,
|
|
omega.y,
|
|
omega.z);
|
|
break;
|
|
}
|
|
|
|
case MSG_LOCATION:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
mavlink_msg_global_position_int_send(
|
|
chan,
|
|
current_loc.lat,
|
|
current_loc.lng,
|
|
/*current_loc.alt * 10,*/ // changed to absolute altitude
|
|
g_gps->altitude,
|
|
g_gps->ground_speed * rot.a.x,
|
|
g_gps->ground_speed * rot.b.x,
|
|
g_gps->ground_speed * rot.c.x);
|
|
break;
|
|
}
|
|
|
|
case MSG_NAV_CONTROLLER_OUTPUT:
|
|
{
|
|
//if (control_mode != MANUAL) {
|
|
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
|
mavlink_msg_nav_controller_output_send(
|
|
chan,
|
|
nav_roll / 1.0e2,
|
|
nav_pitch / 1.0e2,
|
|
target_bearing / 1.0e2,
|
|
target_bearing / 1.0e2,
|
|
wp_distance,
|
|
altitude_error / 1.0e2,
|
|
0,
|
|
0);
|
|
//}
|
|
break;
|
|
}
|
|
|
|
case MSG_LOCAL_LOCATION:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
|
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
mavlink_msg_local_position_send(
|
|
chan,
|
|
timeStamp,
|
|
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
|
|
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
|
|
(current_loc.alt - home.alt) / 1.0e2,
|
|
g_gps->ground_speed / 1.0e2 * rot.a.x,
|
|
g_gps->ground_speed / 1.0e2 * rot.b.x,
|
|
g_gps->ground_speed / 1.0e2 * rot.c.x);
|
|
break;
|
|
}
|
|
|
|
case MSG_GPS_RAW:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
|
mavlink_msg_gps_raw_send(
|
|
chan,
|
|
timeStamp,
|
|
g_gps->status(),
|
|
g_gps->latitude / 1.0e7,
|
|
g_gps->longitude / 1.0e7,
|
|
g_gps->altitude / 100.0,
|
|
g_gps->hdop,
|
|
0.0,
|
|
g_gps->ground_speed / 100.0,
|
|
g_gps->ground_course / 100.0);
|
|
break;
|
|
}
|
|
|
|
case MSG_SERVO_OUT:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
|
uint8_t rssi = 1;
|
|
// normalized values scaled to -10000 to 10000
|
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
|
mavlink_msg_rc_channels_scaled_send(
|
|
chan,
|
|
10000 * g.rc_1.norm_output(),
|
|
10000 * g.rc_2.norm_output(),
|
|
10000 * g.rc_3.norm_output(),
|
|
10000 * g.rc_4.norm_output(),
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
rssi);
|
|
break;
|
|
}
|
|
|
|
case MSG_RADIO_IN:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
|
uint8_t rssi = 1;
|
|
mavlink_msg_rc_channels_raw_send(
|
|
chan,
|
|
g.rc_1.radio_in,
|
|
g.rc_2.radio_in,
|
|
g.rc_3.radio_in,
|
|
g.rc_4.radio_in,
|
|
g.rc_5.radio_in,
|
|
g.rc_6.radio_in,
|
|
g.rc_7.radio_in,
|
|
g.rc_8.radio_in,
|
|
rssi);
|
|
break;
|
|
}
|
|
|
|
case MSG_RADIO_OUT:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
|
mavlink_msg_servo_output_raw_send(
|
|
chan,
|
|
motor_out[0],
|
|
motor_out[1],
|
|
motor_out[2],
|
|
motor_out[3],
|
|
motor_out[4],
|
|
motor_out[5],
|
|
motor_out[6],
|
|
motor_out[7]);
|
|
break;
|
|
}
|
|
|
|
case MSG_VFR_HUD:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
|
mavlink_msg_vfr_hud_send(
|
|
chan,
|
|
(float)airspeed / 100.0,
|
|
(float)g_gps->ground_speed / 100.0,
|
|
(dcm.yaw_sensor / 100) % 360,
|
|
g.rc_3.servo_out/10,
|
|
/*current_loc.alt / 100.0,*/ // changed to absolute altitude
|
|
g_gps->altitude/100.0,
|
|
climb_rate);
|
|
break;
|
|
}
|
|
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
case MSG_RAW_IMU1:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
|
Vector3f accel = imu.get_accel();
|
|
Vector3f gyro = imu.get_gyro();
|
|
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
|
|
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
|
|
mavlink_msg_raw_imu_send(
|
|
chan,
|
|
timeStamp,
|
|
accel.x * 1000.0 / gravity,
|
|
accel.y * 1000.0 / gravity,
|
|
accel.z * 1000.0 / gravity,
|
|
gyro.x * 1000.0,
|
|
gyro.y * 1000.0,
|
|
gyro.z * 1000.0,
|
|
compass.mag_x,
|
|
compass.mag_y,
|
|
compass.mag_z);
|
|
break;
|
|
}
|
|
|
|
case MSG_RAW_IMU2:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
|
mavlink_msg_scaled_pressure_send(
|
|
chan,
|
|
timeStamp,
|
|
(float)barometer.Press/100.0,
|
|
(float)(barometer.Press-ground_pressure)/100.0,
|
|
(int)(barometer.Temp*10));
|
|
break;
|
|
}
|
|
|
|
case MSG_RAW_IMU3:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
|
Vector3f mag_offsets = compass.get_offsets();
|
|
|
|
mavlink_msg_sensor_offsets_send(chan,
|
|
mag_offsets.x,
|
|
mag_offsets.y,
|
|
mag_offsets.z,
|
|
compass.get_declination(),
|
|
barometer.RawPress,
|
|
barometer.RawTemp,
|
|
imu.gx(), imu.gy(), imu.gz(),
|
|
imu.ax(), imu.ay(), imu.az());
|
|
break;
|
|
}
|
|
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
|
|
|
|
case MSG_GPS_STATUS:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
|
mavlink_msg_gps_status_send(
|
|
chan,
|
|
g_gps->num_sats,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL);
|
|
break;
|
|
}
|
|
|
|
case MSG_CURRENT_WAYPOINT:
|
|
{
|
|
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
|
mavlink_msg_waypoint_current_send(
|
|
chan,
|
|
g.waypoint_index);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
|
|
// switch types in mavlink_try_send_message()
|
|
static struct mavlink_queue {
|
|
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
|
|
uint8_t next_deferred_message;
|
|
uint8_t num_deferred_messages;
|
|
} mavlink_queue[2];
|
|
|
|
// send a message using mavlink
|
|
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
|
|
{
|
|
uint8_t i, nextid;
|
|
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
|
|
|
|
// see if we can send the deferred messages, if any
|
|
while (q->num_deferred_messages != 0) {
|
|
if (!mavlink_try_send_message(chan,
|
|
q->deferred_messages[q->next_deferred_message],
|
|
packet_drops)) {
|
|
break;
|
|
}
|
|
q->next_deferred_message++;
|
|
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
|
|
q->next_deferred_message = 0;
|
|
}
|
|
q->num_deferred_messages--;
|
|
}
|
|
|
|
if (id == MSG_RETRY_DEFERRED) {
|
|
return;
|
|
}
|
|
|
|
// this message id might already be deferred
|
|
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
|
|
if (q->deferred_messages[nextid] == id) {
|
|
// its already deferred, discard
|
|
return;
|
|
}
|
|
nextid++;
|
|
if (nextid == MAX_DEFERRED_MESSAGES) {
|
|
nextid = 0;
|
|
}
|
|
}
|
|
|
|
if (q->num_deferred_messages != 0 ||
|
|
!mavlink_try_send_message(chan, id, packet_drops)) {
|
|
// can't send it now, so defer it
|
|
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
|
|
// the defer buffer is full, discard
|
|
return;
|
|
}
|
|
nextid = q->next_deferred_message + q->num_deferred_messages;
|
|
if (nextid >= MAX_DEFERRED_MESSAGES) {
|
|
nextid -= MAX_DEFERRED_MESSAGES;
|
|
}
|
|
q->deferred_messages[nextid] = id;
|
|
q->num_deferred_messages++;
|
|
}
|
|
}
|
|
|
|
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
|
|
{
|
|
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
|
// don't send status MAVLink messages for 1 second after
|
|
// bootup, to try to prevent Xbee bricking
|
|
return;
|
|
}
|
|
mavlink_msg_statustext_send(
|
|
chan,
|
|
severity,
|
|
(const int8_t*) str);
|
|
}
|
|
|
|
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
|
|
{
|
|
}
|
|
|
|
#endif // mavlink in use
|
|
|
|
#endif // inclusion guard
|