Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Changed if statements to switch statement.

Browse Source 
Rover: Changed if statements to switch statement.
This commit is contained in:
murata 2016-12-23 18:49:39 +09:00 committed by Grant Morphett
parent adad75756d
commit 490571ba44
2 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/GCS_Mavlink.cpp
View File

@ -1271,20 +1271,27 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
// prepare and send target position // prepare and send target position
if (!pos_ignore) { if (!pos_ignore) {
Location loc = rover.current_loc; Location loc = rover.current_loc;
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || switch (packet.coordinate_frame) {
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_NED: {
// rotate from body-frame to NE frame // rotate from body-frame to NE frame
float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw(); float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw(); float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
// add offset to current location // add offset to current location
location_offset(loc, ne_x, ne_y); location_offset(loc, ne_x, ne_y);
} else if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED) { }
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location // add offset to current location
location_offset(loc, packet.x, packet.y); location_offset(loc, packet.x, packet.y);
} else { break;
default:
// MAV_FRAME_LOCAL_NED interpret as an offset from home // MAV_FRAME_LOCAL_NED interpret as an offset from home
loc = rover.ahrs.get_home(); loc = rover.ahrs.get_home();
location_offset(loc, packet.x, packet.y); location_offset(loc, packet.x, packet.y);
break;
} }
rover.guided_WP = loc; rover.guided_WP = loc;

16
APMrover2/control_modes.cpp
View File

@ -80,7 +80,8 @@ void Rover::read_trim_switch()
if (ch7_flag) { if (ch7_flag) {
ch7_flag = false; ch7_flag = false;
if (control_mode == MANUAL) { switch (control_mode) {
case MANUAL:
hal.console->println("Erasing waypoints"); hal.console->println("Erasing waypoints");
// if SW7 is ON in MANUAL = Erase the Flight Plan // if SW7 is ON in MANUAL = Erase the Flight Plan
mission.clear(); mission.clear();
@ -89,7 +90,9 @@ void Rover::read_trim_switch()
init_home(); init_home();
} }
return; return;
} else if (control_mode == LEARNING || control_mode == STEERING) {
case LEARNING:
case STEERING: {
// if SW7 is ON in LEARNING = record the Wp // if SW7 is ON in LEARNING = record the Wp
// create new mission command // create new mission command
@ -105,9 +108,16 @@ void Rover::read_trim_switch()
if(mission.add_cmd(cmd)) { if(mission.add_cmd(cmd)) {
hal.console->printf("Learning waypoint %u", (unsigned)mission.num_commands()); hal.console->printf("Learning waypoint %u", (unsigned)mission.num_commands());
} }
} else if (control_mode == AUTO) { }
break;
case AUTO:
// if SW7 is ON in AUTO = set to RTL // if SW7 is ON in AUTO = set to RTL
set_mode(RTL); set_mode(RTL);
break;
default:
break;
} }
} }
} }