From f74c1f9c7eff9e06f90cd57521c91483050aa2f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:35:55 +1000 Subject: [PATCH] MAVLink: merged in the stack saving changes from ArduPlane --- ArduCopter/HIL_Xplane.pde | 2 - ArduCopter/Mavlink_Common.h | 593 +++++++++++++++++++----------------- ArduCopter/defines.h | 1 - 3 files changed, 314 insertions(+), 282 deletions(-) diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde index 311b9da649..9ecf47ab84 100644 --- a/ArduCopter/HIL_Xplane.pde +++ b/ArduCopter/HIL_Xplane.pde @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 6ecb79761f..927289dad6 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -25,14 +25,257 @@ 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) +{ + 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 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); +} + +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, + target_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + 0, + 0); +} + +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.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); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + const 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); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + 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]); +} + +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, + g.rc_3.servo_out/10, + 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-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 @@ -40,298 +283,90 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } switch(id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + break; - 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_STATUS1: - { - CHECK_PAYLOAD_SIZE(SYS_STATUS); + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; - 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; + 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; - 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 + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - 0, - battery_voltage * 1000, - battery_remaining, - packet_drops); - break; - } + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); - break; - } + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + 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_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + 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, // reverted to relative 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_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + 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; - } +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + 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_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + 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_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE - 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_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + 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_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + 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, - /*g_gps->altitude/100.0,*/ // reverted to relative altitude - 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; + default: + break; } return true; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7f1c1dcf62..eca5b9ef6e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -243,7 +243,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1