mirror of https://github.com/ArduPilot/ardupilot
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
parent
e40fe2293b
commit
ee1541cda7
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue