aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APMRover2: correct some style after addition of velocity controler

Browse Source
This commit is contained in:
Pierre Kancir 2017-05-03 16:06:54 +02:00 committed by Grant Morphett
parent ed26c103f9
commit f725e9f2b5
4 changed files with 6 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.cpp
View File

@ -476,7 +476,7 @@ void Rover::update_current_mode(void)
calc_nav_steer(); calc_nav_steer();
calc_throttle(rover.guided_control.target_speed); calc_throttle(rover.guided_control.target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt), Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0)); Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0.0f));
} }
break; break;

6
APMrover2/GCS_Mavlink.cpp
View File

@ -1315,8 +1315,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_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(); const 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(); const 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(target_loc, ne_x, ne_y); location_offset(target_loc, ne_x, ne_y);
} }
@ -1377,7 +1377,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (rover.control_mode != GUIDED) { if (rover.control_mode != GUIDED) {
break; break;
} }
// check for supported coordinate frames // check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
@ -1385,7 +1384,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break; 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;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;

10
APMrover2/commands.cpp
View File

@ -1,14 +1,4 @@
#include "Rover.h" #include "Rover.h"
/* Functions in this file:
void set_next_WP(const AP_Mission::Mission_Command& cmd)
void set_guided_WP(void)
void init_home()
void restart_nav()
************************************************************
*/
/* /*
* set_auto_WP - sets the target location the vehicle should drive to in Auto mode * set_auto_WP - sets the target location the vehicle should drive to in Auto mode
*/ */

7
APMrover2/commands_logic.cpp
View File

@ -379,7 +379,7 @@ void Rover::nav_set_yaw_speed()
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0; lateral_acceleration = 0.0f;
return; return;
} }
@ -389,11 +389,10 @@ void Rover::nav_set_yaw_speed()
// speed param in the message gives speed as a proportion of cruise speed. // speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed // 0.5 would set speed to the cruise speed
// 1 is double the cruise speed. // 1 is double the cruise speed.
float target_speed = g.speed_cruise * guided_control.target_speed * 2; const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
calc_throttle(target_speed); calc_throttle(target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0, 0), Vector3f(target_speed, 0, 0)); Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
return;
} }
void Rover::nav_set_speed() void Rover::nav_set_speed()