From ee1541cda77ee9d49fc31211feee14b11731e17e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:05:21 +1000 Subject: [PATCH] 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! --- ArduPlane/Mavlink_Common.h | 675 ++++++++++++++++++++----------------- 1 file changed, 368 insertions(+), 307 deletions(-) diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..9923ab8f98 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,327 +25,388 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + 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 + */ + +#define NOINLINE __attribute__((noinline)) + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + 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 nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + break; + case FLY_BY_WIRE_A: + mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case FLY_BY_WIRE_B: + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + 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 LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_LOITER; + break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; + } + + 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, + load * 1000, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + 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, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_local_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_local_position_send( + chan, + micros(), + 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); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + 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); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const 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.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + (int)g.channel_throttle.servo_out, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + 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); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + 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()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + + // 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) { + switch (id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; - 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); + 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; - 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 MANUAL: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_TEST1; - break; - case FLY_BY_WIRE_A: - mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - 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 LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LOITER; - break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; - } - - 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, - load * 1000, - 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: - { - 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 - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - 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, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - airspeed_error, - crosstrack_error); - } - 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.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.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.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - 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, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); - 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, - (int)g.channel_throttle.servo_out, - current_loc.alt / 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-g.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; + default: + break; } return true; }