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

rework the MAVLink send code to avoid excessive stack usage 

this avoids a varient of the gcc excessive stack usage bug, by
wrapping the send calls in NOINLINE functions. This saves us a lot of
stack space, and strangely enough produces slightly smaller code!
This commit is contained in:
Andrew Tridgell 2011-09-18 11:05:21 +10:00
parent e40fe2293b
commit ee1541cda7
1 changed files with 368 additions and 307 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

233
ArduPlane/Mavlink_Common.h
View File

@ -25,36 +25,44 @@ 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); Vector3f omega = dcm.get_gyro();
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;
@ -106,36 +114,16 @@ 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)
{ {
CHECK_PAYLOAD_SIZE(ATTITUDE);
Vector3f omega = dcm.get_gyro();
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,
@ -145,13 +133,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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,
@ -163,31 +148,26 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
airspeed_error, airspeed_error,
crosstrack_error); crosstrack_error);
} }
break;
}
case MSG_LOCAL_LOCATION: static void NOINLINE send_local_location(mavlink_channel_t chan)
{ {
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
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_local_position_send( mavlink_msg_local_position_send(
chan, chan,
timeStamp, micros(),
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, 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)), ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
(current_loc.alt - home.alt) / 1.0e2, (current_loc.alt - home.alt) / 1.0e2,
g_gps->ground_speed / 1.0e2 * rot.a.x, 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.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x); g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
} }
case MSG_GPS_RAW: static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{ {
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,
@ -196,13 +176,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(
@ -216,12 +194,10 @@ 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);
uint8_t rssi = 1; uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send( mavlink_msg_rc_channels_raw_send(
chan, chan,
@ -234,12 +210,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,
g.channel_roll.radio_out, g.channel_roll.radio_out,
@ -250,12 +224,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
g.rc_6.radio_out, g.rc_6.radio_out,
g.rc_7.radio_out, g.rc_7.radio_out,
g.rc_8.radio_out); g.rc_8.radio_out);
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,
@ -264,21 +236,17 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
(int)g.channel_throttle.servo_out, (int)g.channel_throttle.servo_out,
current_loc.alt / 100.0, current_loc.alt / 100.0,
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,
@ -288,24 +256,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-g.ground_pressure)/100.0, (float)(barometer.Press-g.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,
@ -317,13 +281,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,
@ -332,18 +294,117 @@ 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);
return true;
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:
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
}
break;
case MSG_LOCAL_LOCATION:
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
send_local_location(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;
} }