Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

MAVLink: merged in the stack saving changes from ArduPlane

This commit is contained in:
Andrew Tridgell 2011-09-18 11:35:55 +10:00
parent 87ddd50c2a
commit dc23ad4275
3 changed files with 314 additions and 282 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/HIL_Xplane.pde
View File

@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
break; break;
case MSG_LOCATION: case MSG_LOCATION:
break; break;
case MSG_LOCAL_LOCATION:
break;
case MSG_GPS_RAW: case MSG_GPS_RAW:
break; break;
case MSG_SERVO_OUT: case MSG_SERVO_OUT:

241
ArduCopter/Mavlink_Common.h
View File

@ -25,36 +25,43 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
} }
} }
// 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 #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 !!NOTE!!
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) { the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
case MSG_HEARTBEAT: #define NOINLINE __attribute__((noinline))
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send( mavlink_msg_heartbeat_send(
chan, chan,
mavlink_system.type, mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA); MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
} }
case MSG_EXTENDED_STATUS1: static NOINLINE void send_attitude(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(SYS_STATUS); mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT; uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR; uint8_t nav_mode = MAV_NAV_VECTOR;
@ -76,7 +83,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
break; break;
default: default:
mode = control_mode + 100; mode = control_mode + 100;
} }
uint8_t status = MAV_STATE_ACTIVE; uint8_t status = MAV_STATE_ACTIVE;
@ -91,53 +97,29 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
battery_voltage * 1000, battery_voltage * 1000,
battery_remaining, battery_remaining,
packet_drops); packet_drops);
break;
} }
case MSG_EXTENDED_STATUS2: static void NOINLINE send_meminfo(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval; extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break;
} }
case MSG_ATTITUDE: static void NOINLINE send_location(mavlink_channel_t chan)
{ {
//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 Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send( mavlink_msg_global_position_int_send(
chan, chan,
current_loc.lat, current_loc.lat,
current_loc.lng, current_loc.lng,
current_loc.alt * 10, // reverted to relative altitude current_loc.alt * 10,
/*g_gps->altitude,*/
g_gps->ground_speed * rot.a.x, g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x, g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x); g_gps->ground_speed * rot.c.x);
break;
} }
case MSG_NAV_CONTROLLER_OUTPUT: static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{ {
//if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_roll / 1.0e2, nav_roll / 1.0e2,
@ -148,32 +130,13 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
altitude_error / 1.0e2, altitude_error / 1.0e2,
0, 0,
0); 0);
//}
break;
} }
case MSG_LOCAL_LOCATION: static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{ {
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( mavlink_msg_gps_raw_send(
chan, chan,
timeStamp, micros(),
g_gps->status(), g_gps->status(),
g_gps->latitude / 1.0e7, g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7, g_gps->longitude / 1.0e7,
@ -182,13 +145,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
0.0, 0.0,
g_gps->ground_speed / 100.0, g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0); g_gps->ground_course / 100.0);
break;
} }
case MSG_SERVO_OUT: static void NOINLINE send_servo_out(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); const uint8_t rssi = 1;
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000 // normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers // This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send( mavlink_msg_rc_channels_scaled_send(
@ -202,13 +163,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
0, 0,
0, 0,
rssi); rssi);
break;
} }
case MSG_RADIO_IN: static void NOINLINE send_radio_in(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); const uint8_t rssi = 1;
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send( mavlink_msg_rc_channels_raw_send(
chan, chan,
g.rc_1.radio_in, g.rc_1.radio_in,
@ -220,12 +179,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
g.rc_7.radio_in, g.rc_7.radio_in,
g.rc_8.radio_in, g.rc_8.radio_in,
rssi); rssi);
break;
} }
case MSG_RADIO_OUT: static void NOINLINE send_radio_out(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send( mavlink_msg_servo_output_raw_send(
chan, chan,
motor_out[0], motor_out[0],
@ -236,12 +193,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
motor_out[5], motor_out[5],
motor_out[6], motor_out[6],
motor_out[7]); motor_out[7]);
break;
} }
case MSG_VFR_HUD: static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send( mavlink_msg_vfr_hud_send(
chan, chan,
(float)airspeed / 100.0, (float)airspeed / 100.0,
@ -249,22 +204,17 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
(dcm.yaw_sensor / 100) % 360, (dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10, g.rc_3.servo_out/10,
current_loc.alt / 100.0, current_loc.alt / 100.0,
/*g_gps->altitude/100.0,*/ // reverted to relative altitude
climb_rate); climb_rate);
break;
} }
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1: static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro(); 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( mavlink_msg_raw_imu_send(
chan, chan,
timeStamp, micros(),
accel.x * 1000.0 / gravity, accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity, accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity, accel.z * 1000.0 / gravity,
@ -274,24 +224,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,
compass.mag_z); compass.mag_z);
break;
} }
case MSG_RAW_IMU2: static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send( mavlink_msg_scaled_pressure_send(
chan, chan,
timeStamp, micros(),
(float)barometer.Press/100.0, (float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0, (float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10)); (int)(barometer.Temp*10));
break;
} }
case MSG_RAW_IMU3: static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
Vector3f mag_offsets = compass.get_offsets(); Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan, mavlink_msg_sensor_offsets_send(chan,
@ -303,13 +249,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
barometer.RawTemp, barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(), imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az()); imu.ax(), imu.ay(), imu.az());
break;
} }
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE #endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS: static void NOINLINE send_gps_status(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send( mavlink_msg_gps_status_send(
chan, chan,
g_gps->num_sats, g_gps->num_sats,
@ -318,18 +262,109 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
NULL, NULL,
NULL, NULL,
NULL); NULL);
break;
} }
case MSG_CURRENT_WAYPOINT: static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send( mavlink_msg_waypoint_current_send(
chan, chan,
g.waypoint_index); g.waypoint_index);
break;
} }
// 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)
{
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
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);
send_heartbeat(chan);
break;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
default: default:
break; break;
} }

1
ArduCopter/defines.h
View File

@ -243,7 +243,6 @@
#define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2 #define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3 #define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff #define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1 #define SEVERITY_LOW 1