mirror of https://github.com/ArduPilot/ardupilot
Rover: correct indentation and style on GCS_Mavlink
Remove trailling whitespace, tabs, limit single line if-statement scope, add missing space
This commit is contained in:
Pierre Kancir
Grant Morphett
parent
326e0f224f
commit
5ed9d22bf6
|
|
@ -8,7 +8,7 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
|
|||
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
||||
uint8_t system_status = MAV_STATE_ACTIVE;
|
||||
uint32_t custom_mode = control_mode;
|
||||
|
||||
|
||||
if (failsafe.triggered != 0) {
|
||||
system_status = MAV_STATE_CRITICAL;
|
||||
}
|
||||
|
|
@ -43,7 +43,7 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
|
|||
break;
|
||||
}
|
||||
|
||||
#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING==ENABLED)
|
||||
#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED)
|
||||
if (control_mode != INITIALISING) {
|
||||
// all modes except INITIALISING have some form of manual
|
||||
// override if stick mixing is enabled
|
||||
|
|
@ -101,11 +101,11 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
|
|||
control_sensors_enabled,
|
||||
control_sensors_health,
|
||||
(uint16_t)(scheduler.load_average(20000) * 1000),
|
||||
battery.voltage() * 1000, // mV
|
||||
battery.voltage() * 1000, // mV
|
||||
battery_current, // in 10mA units
|
||||
battery_remaining, // in %
|
||||
0, // comm drops %,
|
||||
0, // comm drops in pkts,
|
||||
0, // comm drops %,
|
||||
0, // comm drops in pkts,
|
||||
0, 0, 0, 0);
|
||||
}
|
||||
|
||||
|
|
@ -116,7 +116,7 @@ void Rover::send_location(mavlink_channel_t chan)
|
|||
// allows us to correctly calculate velocities and extrapolate
|
||||
// positions.
|
||||
// If we don't have a GPS fix then we are dead reckoning, and will
|
||||
// use the current boot time as the fix time.
|
||||
// use the current boot time as the fix time.
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
|
||||
fix_time = gps.last_fix_time_ms();
|
||||
} else {
|
||||
|
|
@ -126,13 +126,13 @@ void Rover::send_location(mavlink_channel_t chan)
|
|||
mavlink_msg_global_position_int_send(
|
||||
chan,
|
||||
fix_time,
|
||||
current_loc.lat, // in 1E7 degrees
|
||||
current_loc.lng, // in 1E7 degrees
|
||||
current_loc.alt * 10UL, // millimeters above sea level
|
||||
(current_loc.alt - home.alt) * 10, // millimeters above ground
|
||||
vel.x * 100, // X speed cm/s (+ve North)
|
||||
vel.y * 100, // Y speed cm/s (+ve East)
|
||||
vel.z * -100, // Z speed cm/s (+ve up)
|
||||
current_loc.lat, // in 1E7 degrees
|
||||
current_loc.lng, // in 1E7 degrees
|
||||
current_loc.alt * 10UL, // millimeters above sea level
|
||||
(current_loc.alt - home.alt) * 10, // millimeters above ground
|
||||
vel.x * 100, // X speed cm/s (+ve North)
|
||||
vel.y * 100, // Y speed cm/s (+ve East)
|
||||
vel.z * -100, // Z speed cm/s (+ve up)
|
||||
ahrs.yaw_sensor);
|
||||
}
|
||||
|
||||
|
|
@ -140,8 +140,8 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
|
|||
{
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
lateral_acceleration, // use nav_roll to hold demanded Y accel
|
||||
ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
|
||||
lateral_acceleration, // use nav_roll to hold demanded Y accel
|
||||
ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
|
||||
nav_controller->nav_bearing_cd() * 0.01f,
|
||||
nav_controller->target_bearing_cd() * 0.01f,
|
||||
wp_distance,
|
||||
|
|
@ -159,7 +159,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
|
|||
mavlink_msg_rc_channels_scaled_send(
|
||||
chan,
|
||||
millis(),
|
||||
0, // port 0
|
||||
0, // port 0
|
||||
10000 * channel_steer->norm_output(),
|
||||
0,
|
||||
10000 * channel_throttle->norm_output(),
|
||||
|
|
@ -248,7 +248,7 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
|
|||
const DataFlash_Class::PID_Info *pid_info;
|
||||
if (g.gcs_pid_mask & 1) {
|
||||
pid_info = &steerController.get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
|
||||
pid_info->desired,
|
||||
degrees(gyro.z),
|
||||
pid_info->FF,
|
||||
|
|
@ -261,7 +261,7 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
|
|||
}
|
||||
if (g.gcs_pid_mask & 2) {
|
||||
pid_info = &g.pidSpeedThrottle.get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
|
||||
pid_info->desired,
|
||||
0,
|
||||
0,
|
||||
|
|
@ -302,7 +302,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
switch (id) {
|
||||
case MSG_HEARTBEAT:
|
||||
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
||||
last_heartbeat_time = AP_HAL::millis();
|
||||
last_heartbeat_time = AP_HAL::millis();
|
||||
rover.send_heartbeat(chan);
|
||||
return true;
|
||||
|
||||
|
|
@ -423,7 +423,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
#if MOUNT == ENABLED
|
||||
CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
|
||||
rover.camera_mount.status_msg(chan);
|
||||
#endif // MOUNT == ENABLED
|
||||
#endif // MOUNT == ENABLED
|
||||
break;
|
||||
|
||||
case MSG_RAW_IMU2:
|
||||
|
|
@ -482,11 +482,9 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
case MSG_GIMBAL_REPORT:
|
||||
case MSG_RPM:
|
||||
case MSG_POSITION_TARGET_GLOBAL_INT:
|
||||
break; // just here to prevent a warning
|
||||
break; // just here to prevent a warning
|
||||
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -595,7 +593,9 @@ GCS_MAVLINK_Rover::data_stream_send(void)
|
|||
}
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (rover.in_mavlink_delay) {
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
|
|
@ -613,14 +613,18 @@ GCS_MAVLINK_Rover::data_stream_send(void)
|
|||
return;
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RAW_SENSORS)) {
|
||||
send_message(MSG_RAW_IMU1);
|
||||
send_message(MSG_RAW_IMU3);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
|
||||
send_message(MSG_EXTENDED_STATUS1);
|
||||
|
|
@ -630,7 +634,9 @@ GCS_MAVLINK_Rover::data_stream_send(void)
|
|||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_POSITION)) {
|
||||
// sent with GPS read
|
||||
|
|
@ -638,20 +644,26 @@ GCS_MAVLINK_Rover::data_stream_send(void)
|
|||
send_message(MSG_LOCAL_POSITION);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
|
||||
send_message(MSG_SERVO_OUT);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_RC_CHANNELS)) {
|
||||
send_message(MSG_RADIO_OUT);
|
||||
send_message(MSG_RADIO_IN);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA1)) {
|
||||
send_message(MSG_ATTITUDE);
|
||||
|
|
@ -660,14 +672,18 @@ GCS_MAVLINK_Rover::data_stream_send(void)
|
|||
send_message(MSG_PID_TUNING);
|
||||
}
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA2)) {
|
||||
send_message(MSG_VFR_HUD);
|
||||
}
|
||||
|
||||
if (rover.gcs_out_of_time) return;
|
||||
if (rover.gcs_out_of_time) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA3)) {
|
||||
send_message(MSG_AHRS);
|
||||
|
|
@ -725,7 +741,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
mavlink_statustext_t packet;
|
||||
mavlink_msg_statustext_decode(msg, &packet);
|
||||
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G','C','S',':'};
|
||||
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G', 'C', 'S', ':'};
|
||||
memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
|
||||
rover.DataFlash.Log_Write_Message(text);
|
||||
break;
|
||||
|
|
@ -737,7 +753,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_msg_command_int_decode(msg, &packet);
|
||||
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
||||
|
||||
switch(packet.command) {
|
||||
switch (packet.command) {
|
||||
#if MOUNT == ENABLED
|
||||
case MAV_CMD_DO_SET_ROI: {
|
||||
// param1 : /* Region of interest mode (not used)*/
|
||||
|
|
@ -789,7 +805,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
|
||||
// do command
|
||||
|
||||
switch(packet.command) {
|
||||
switch (packet.command) {
|
||||
|
||||
case MAV_CMD_START_RX_PAIR:
|
||||
result = handle_rc_bind(packet);
|
||||
|
|
@ -848,7 +864,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
#endif // CAMERA == ENABLED
|
||||
#endif // CAMERA == ENABLED
|
||||
|
||||
case MAV_CMD_DO_MOUNT_CONTROL:
|
||||
#if MOUNT == ENABLED
|
||||
|
|
@ -863,11 +879,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
|
||||
case MAV_CMD_PREFLIGHT_CALIBRATION:
|
||||
if(hal.util->get_soft_armed()) {
|
||||
if (hal.util->get_soft_armed()) {
|
||||
result = MAV_RESULT_FAILED;
|
||||
break;
|
||||
}
|
||||
if (is_equal(packet.param1,1.0f)) {
|
||||
if (is_equal(packet.param1, 1.0f)) {
|
||||
rover.ins.init_gyro();
|
||||
if (rover.ins.gyro_calibrated_ok_all()) {
|
||||
rover.ahrs.reset_gyro_drift();
|
||||
|
|
@ -875,13 +891,13 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
} else if (is_equal(packet.param3,1.0f)) {
|
||||
} else if (is_equal(packet.param3, 1.0f)) {
|
||||
rover.init_barometer(false);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else if (is_equal(packet.param4,1.0f)) {
|
||||
} else if (is_equal(packet.param4, 1.0f)) {
|
||||
rover.trim_radio();
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else if (is_equal(packet.param5,1.0f)) {
|
||||
} else if (is_equal(packet.param5, 1.0f)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
// start with gyro calibration
|
||||
rover.ins.init_gyro();
|
||||
|
|
@ -894,20 +910,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
rover.ins.acal_init();
|
||||
rover.ins.get_acal()->start(this);
|
||||
|
||||
} else if (is_equal(packet.param5,2.0f)) {
|
||||
} else if (is_equal(packet.param5, 2.0f)) {
|
||||
// start with gyro calibration
|
||||
rover.ins.init_gyro();
|
||||
// accel trim
|
||||
float trim_roll, trim_pitch;
|
||||
if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) {
|
||||
if (rover.ins.calibrate_trim(trim_roll, trim_pitch)) {
|
||||
// reset ahrs's trim to suggested values from calibration routine
|
||||
rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
|
||||
}
|
||||
break;
|
||||
|
|
@ -979,15 +994,15 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
|
||||
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
||||
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
|
||||
if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) {
|
||||
// when packet.param1 == 3 we reboot to hold in bootloader
|
||||
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
|
||||
hal.scheduler->reboot(is_equal(packet.param1, 3.0f));
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_COMPONENT_ARM_DISARM:
|
||||
if (is_equal(packet.param1,1.0f)) {
|
||||
if (is_equal(packet.param1, 1.0f)) {
|
||||
// run pre_arm_checks and arm_checks and display failures
|
||||
if (rover.arm_motors(AP_Arming::MAVLINK)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
|
|
@ -1015,7 +1030,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
|
||||
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
|
||||
if (is_equal(packet.param1,1.0f)) {
|
||||
if (is_equal(packet.param1, 1.0f)) {
|
||||
send_autopilot_version(FIRMWARE_VERSION);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
|
|
@ -1028,8 +1043,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
// param5 : latitude
|
||||
// param6 : longitude
|
||||
// param7 : altitude
|
||||
result = MAV_RESULT_FAILED; // assume failure
|
||||
if (is_equal(packet.param1,1.0f)) {
|
||||
result = MAV_RESULT_FAILED; // assume failure
|
||||
if (is_equal(packet.param1, 1.0f)) {
|
||||
rover.init_home();
|
||||
} else {
|
||||
if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
|
||||
|
|
@ -1050,9 +1065,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
GCS_MAVLINK::send_home_all(new_home_loc);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
|
||||
(double)(new_home_loc.lat*1.0e-7f),
|
||||
(double)(new_home_loc.lng*1.0e-7f),
|
||||
(uint32_t)(new_home_loc.alt*0.01f));
|
||||
(double)(new_home_loc.lat * 1.0e-7f),
|
||||
(double)(new_home_loc.lng * 1.0e-7f),
|
||||
(uint32_t)(new_home_loc.alt * 0.01f));
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1105,7 +1120,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
|
||||
case MAVLINK_MSG_ID_SET_MODE:
|
||||
{
|
||||
{
|
||||
handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
|
||||
break;
|
||||
}
|
||||
|
|
@ -1117,7 +1132,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
|
||||
|
||||
// XXX read a WP from EEPROM and send it to the GCS
|
||||
// XXX read a WP from EEPROM and send it to the GCS
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||
{
|
||||
|
|
@ -1196,7 +1211,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
// allow override of RC channel values for HIL
|
||||
// or for complete GCS control of switch position
|
||||
// and RC PWM values.
|
||||
if(msg->sysid != rover.g.sysid_my_gcs) break; // Only accept control from our gcs
|
||||
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
|
||||
break;
|
||||
}
|
||||
|
||||
mavlink_rc_channels_override_t packet;
|
||||
int16_t v[8];
|
||||
mavlink_msg_rc_channels_override_decode(msg, &packet);
|
||||
|
|
@ -1220,7 +1238,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
{
|
||||
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
|
||||
if(msg->sysid != rover.g.sysid_my_gcs) break;
|
||||
if (msg->sysid != rover.g.sysid_my_gcs) {
|
||||
break;
|
||||
}
|
||||
|
||||
rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis();
|
||||
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
|
||||
break;
|
||||
|
|
@ -1293,7 +1314,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
}
|
||||
|
||||
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
|
||||
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
|
||||
|
||||
// prepare and send target position
|
||||
if (!pos_ignore) {
|
||||
|
|
@ -1316,11 +1337,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
case MAVLINK_MSG_ID_HIL_STATE:
|
||||
{
|
||||
mavlink_hil_state_t packet;
|
||||
mavlink_msg_hil_state_decode(msg, &packet);
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_STATE:
|
||||
{
|
||||
mavlink_hil_state_t packet;
|
||||
mavlink_msg_hil_state_decode(msg, &packet);
|
||||
|
||||
// sanity check location
|
||||
if (!check_latlng(packet.lat, packet.lon)) {
|
||||
break;
|
||||
|
|
@ -1333,12 +1354,12 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
loc.alt = packet.alt/10;
|
||||
Vector3f vel(packet.vx, packet.vy, packet.vz);
|
||||
vel *= 0.01f;
|
||||
|
||||
|
||||
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
|
||||
packet.time_usec/1000,
|
||||
loc, vel, 10, 0);
|
||||
|
||||
// rad/sec
|
||||
|
||||
// rad/sec
|
||||
Vector3f gyros;
|
||||
gyros.x = packet.rollspeed;
|
||||
gyros.y = packet.pitchspeed;
|
||||
|
|
@ -1349,47 +1370,47 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
|
||||
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
|
||||
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
|
||||
|
||||
|
||||
ins.set_gyro(0, gyros);
|
||||
|
||||
ins.set_accel(0, accels);
|
||||
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
|
||||
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
|
||||
break;
|
||||
}
|
||||
#endif // HIL_MODE
|
||||
}
|
||||
#endif // HIL_MODE
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
// deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
// deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
|
||||
{
|
||||
rover.camera.control_msg(msg);
|
||||
rover.log_picture();
|
||||
break;
|
||||
}
|
||||
#endif // CAMERA == ENABLED
|
||||
#endif // CAMERA == ENABLED
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
|
||||
// deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
|
||||
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
|
||||
{
|
||||
rover.camera_mount.configure_msg(msg);
|
||||
break;
|
||||
}
|
||||
{
|
||||
rover.camera_mount.configure_msg(msg);
|
||||
break;
|
||||
}
|
||||
|
||||
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
|
||||
// deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
|
||||
case MAVLINK_MSG_ID_MOUNT_CONTROL:
|
||||
{
|
||||
rover.camera_mount.control_msg(msg);
|
||||
break;
|
||||
}
|
||||
#endif // MOUNT == ENABLED
|
||||
{
|
||||
rover.camera_mount.control_msg(msg);
|
||||
break;
|
||||
}
|
||||
#endif // MOUNT == ENABLED
|
||||
|
||||
case MAVLINK_MSG_ID_RADIO:
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS:
|
||||
|
|
@ -1448,8 +1469,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
handle_common_message(msg);
|
||||
break;
|
||||
|
||||
} // end switch
|
||||
} // end handle mavlink
|
||||
} // end switch
|
||||
} // end handle mavlink
|
||||
|
||||
/*
|
||||
* a delay() callback that processes MAVLink packets. We set this as the
|
||||
|
|
@ -1460,7 +1481,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
void Rover::mavlink_delay_cb()
|
||||
{
|
||||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
if (!gcs[0].initialised || in_mavlink_delay) return;
|
||||
if (!gcs[0].initialised || in_mavlink_delay) {
|
||||
return;
|
||||
}
|
||||
|
||||
in_mavlink_delay = true;
|
||||
|
||||
|
|
@ -1573,11 +1596,11 @@ void Rover::gcs_retry_deferred(void)
|
|||
*/
|
||||
bool GCS_MAVLINK_Rover::accept_packet(const mavlink_status_t &status, mavlink_message_t &msg)
|
||||
{
|
||||
if (!rover.g2.sysid_enforce) {
|
||||
return true;
|
||||
}
|
||||
if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
|
||||
return true;
|
||||
}
|
||||
return (msg.sysid == rover.g.sysid_my_gcs);
|
||||
}
|
||||
if (!rover.g2.sysid_enforce) {
|
||||
return true;
|
||||
}
|
||||
if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
|
||||
return true;
|
||||
}
|
||||
return (msg.sysid == rover.g.sysid_my_gcs);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue