Home        Store        Docs        Blog

Simulating Manual Control using Mavros


(Patrick José Pereira) #21

Without a GPS the motors will run in max speed, you’ll need something to give some velocity feedback to the ROV.
Are you setting the ROV speed or the motor pwm ?

I’ll investigate the set mode time out.


(MavNav) #22

Im just changing the values in the command you gave me. I’m guessing those are pwm values?
How do i give velocity feedback? I just have a simple setup of:

Computer > Pixhawk > ESC > motor.

Yes please do, I really need this to work so i can progress (I have a deadline with this >.<). Thanks!


(Patrick José Pereira) #23

Ok, try first with mavproxy to see if your setup is correct.

  1. Connect the ROV in your computer and wait for 1~2 minutes to finish the initialization.

  2. Start mavproxy. mavproxy.py --master=/dev/ttyACM0

  3. Use status to see if everything is working. status
    You’ll have this feedback.

32: AHRS {omegaIx : 0.0389807932079, omegaIy : -0.0134896915406, omegaIz : 0.0616827979684, accel_weight : 0.0, renorm_val : 0.0, error_rp : 0.00437930645421, error_yaw : 0.15694104135}
31: AHRS2 {roll : -1.58072090149, pitch : -0.0987562090158, yaw : -0.525687396526, altitude : 0.0, lat : 0, lng : 0}
31: AHRS3 {roll : -1.47963392735, pitch : -0.104524955153, yaw : 2.29476451874, altitude : 0.0, lat : 0, lng : 0, v1 : 0.0, v2 : 0.0, v3 : 0.0, v4 : 0.0}
31: ATTITUDE {time_boot_ms : 463779, roll : -1.47963392735, pitch : -0.104524955153, yaw : 2.29476451874, rollspeed : -7.24875135347e-05, pitchspeed : 0.0206718333066, yawspeed : 0.024894239381}
27: BAD_DATA {Bad prefix, data:['16']}
31: BATTERY_STATUS {id : 0, battery_function : 0, type : 0, temperature : 32767, voltages : [65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535], current_battery : 4, current_consumed : 6, energy_consumed : -1, battery_remaining : 99}
31: EKF_STATUS_REPORT {flags : 0, velocity_variance : 0.0, pos_horiz_variance : 0.0405917428434, pos_vert_variance : 0.0234755519778, compass_variance : 0.0247390381992, terrain_alt_variance : 0.0}
31: GLOBAL_POSITION_INT {time_boot_ms : 463579, lat : 0, lon : 0, alt : -2850, relative_alt : -2850, vx : -8, vy : 15, vz : -19, hdg : 13148}
7: GPS_RAW_INT {time_usec : 0, fix_type : 0, lat : 0, lon : 0, alt : 0, eph : 9999, epv : 0, vel : 0, cog : 0, satellites_visible : 0}
8: HEARTBEAT {type : 12, autopilot : 3, base_mode : 81, custom_mode : 19, system_status : 5, mavlink_version : 3}
32: HWSTATUS {Vcc : 4946, I2Cerr : 0}
32: MEMINFO {brkval : 0, freemem : 9344}
32: MISSION_CURRENT {seq : 0}
31: MOUNT_STATUS {target_system : 0, target_component : 0, pointing_a : 0, pointing_b : 0, pointing_c : 0}
240: NAMED_VALUE_FLOAT {time_boot_ms : 463699, name : InputHold, value : 0.0}
32: NAV_CONTROLLER_OUTPUT {nav_roll : -84.7731704712, nav_pitch : -5.99635505676, nav_bearing : 131, target_bearing : 90, wp_dist : 0, alt_error : 2.85751223564, aspd_error : 0.0, xtrack_error : 0.0}
689: PARAM_VALUE {param_id : RC16_DZ, param_value : 0.0, param_type : 4, param_count : 688, param_index : 687}
32: POWER_STATUS {Vcc : 4946, Vservo : 21, flags : 4}
31: RAW_IMU {time_usec : 463719568, xacc : -97, yacc : 1111, zacc : -62, xgyro : -1, ygyro : 21, zgyro : 27, xmag : -40, ymag : -270, zmag : -56}
32: RC_CHANNELS {time_boot_ms : 463619, chancount : 0, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1500, chan6_raw : 1500, chan7_raw : 0, chan8_raw : 1500, chan9_raw : 1100, chan10_raw : 1100, chan11_raw : 1100, chan12_raw : 0, chan13_raw : 0, chan14_raw : 0, chan15_raw : 0, chan16_raw : 0, chan17_raw : 0, chan18_raw : 0, rssi : 0}
32: RC_CHANNELS_RAW {time_boot_ms : 463619, port : 0, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1500, chan6_raw : 1500, chan7_raw : 0, chan8_raw : 1500, rssi : 0}
31: SCALED_IMU2 {time_boot_ms : 463719, xacc : -86, yacc : 985, zacc : 9, xgyro : -44, ygyro : 13, zgyro : -58, xmag : 0, ymag : 0, zmag : 0}
31: SCALED_PRESSURE {time_boot_ms : 463719, press_abs : 1010.05987549, press_diff : 0.180546864867, temperature : 4318}
3: SENSOR_OFFSETS {mag_ofs_x : 0, mag_ofs_y : 0, mag_ofs_z : 0, mag_declination : 0.0, raw_press : 101004, raw_temp : 4317, gyro_cal_x : 0.0, gyro_cal_y : 0.0, gyro_cal_z : 0.0, accel_cal_x : 0.0, accel_cal_y : 0.0, accel_cal_z : 0.0}
32: SERVO_OUTPUT_RAW {time_usec : 463619511, port : 0, servo1_raw : 1500, servo2_raw : 1500, servo3_raw : 1500, servo4_raw : 1500, servo5_raw : 1500, servo6_raw : 1500, servo7_raw : 1100, servo8_raw : 1500}
4: STATUSTEXT {severity : 6, text : PX4v2 0043001E 33365109 36353335}
64: SYSTEM_TIME {time_unix_usec : 0, time_boot_ms : 463699}
32: SYS_STATUS {onboard_control_sensors_present : 2161671, onboard_control_sensors_enabled : 2137095, onboard_control_sensors_health : 64519, load : 301, voltage_battery : 34, current_battery : 4, battery_remaining : -1, drop_rate_comm : 0, errors_comm : 0, errors_count1 : 0, errors_count2 : 0, errors_count3 : 0, errors_count4 : 0}
31: VFR_HUD {airspeed : 0.0, groundspeed : 0.0, heading : 131, throttle : 0, alt : -2.84999990463, climb : -0.209999993443}
31: VIBRATION {time_usec : 463700236, vibration_x : 0.0813180804253, vibration_y : 0.0598611645401, vibration_z : 0.0537432022393, clipping_0 : 0, clipping_1 : 0, clipping_2 : 0}
  1. arm the ROV. arm throttle
    You’ll receive this message.
MANUAL> Got MAVLink msg: COMMAND_ACK {command : 400, result : 0}
ARMED
  1. Active guided mode. mode GUIDED

If the result message is different from 0, than you’ll not be able to use this mode.
You have probably something wrong with your position system, check the status for GLOBAL_POSITION_ and GPS data.


(MavNav) #24

I do get exactly “MANUAL> Got MAVLink msg: COMMAND_ACK {command : 400, result : 0}
ARMED”

When i do “mode GUIDED”. i get: MANUAL> Got MAVLink msg: COMMAND_ACK {command : 11, result : 4}

This is the status message:

mode GUIDED
MANUAL> Got MAVLink msg: COMMAND_ACK {command : 11, result : 4}
status
MANUAL> Counters: Slave:0 MasterIn:[12831] FGearOut:0 FGearIn:0 MasterOut:97 
MAV Errors: 1
None
385: AHRS {omegaIx : 2.66128645308e-05, omegaIy : -0.00358906248584, omegaIz : 0.00239131599665, accel_weight : 0.0, renorm_val : 0.0, error_rp : 0.00210651196539, error_yaw : 0.00067939190194}
399: AHRS2 {roll : 0.0101025439799, pitch : 0.00602368498221, yaw : 1.28306615353, altitude : 0.0, lat : 0, lng : 0}
399: AHRS3 {roll : 0.0122073432431, pitch : 0.00492259114981, yaw : 1.11974823475, altitude : 0.0, lat : 0, lng : 0, v1 : 0.0, v2 : 0.0, v3 : 0.0, v4 : 0.0}
399: ATTITUDE {time_boot_ms : 399352, roll : 0.0122073432431, pitch : 0.00492259114981, yaw : 1.11974823475, rollspeed : 0.000256764644291, pitchspeed : 0.00138947030064, yawspeed : 0.00155384617392}
7: BAD_DATA {invalid MAVLink CRC in msgID 163 0x0ebe should be 0x30b3, data:['fe', '1c', 'd5', '1', '1', 'a3', '4', '44', 'bf', '39', '41', '7', '7d', 'bb', '67', '72', '4e', 'b9', '0', 'fe', '9', '53', '1', '1', '0', '13', '0', '0', '0', 'c', '3', '51', '5', '3', 'be', 'e']}
3: COMMAND_ACK {command : 11, result : 4}
386: EKF_STATUS_REPORT {flags : 485, velocity_variance : 0.0, pos_horiz_variance : 0.00347328209318, pos_vert_variance : 0.00457791937515, compass_variance : 0.186648115516, terrain_alt_variance : 0.00838069431484}
399: GLOBAL_POSITION_INT {time_boot_ms : 399132, lat : 0, lon : 0, alt : -1440, relative_alt : -1440, vx : -2, vy : 9, vz : -2, hdg : 6415}
82: GPS_RAW_INT {time_usec : 0, fix_type : 0, lat : 0, lon : 0, alt : 0, eph : 9999, epv : 0, vel : 0, cog : 0, satellites_visible : 0}
100: HEARTBEAT {type : 12, autopilot : 3, base_mode : 209, custom_mode : 19, system_status : 5, mavlink_version : 3}
385: HWSTATUS {Vcc : 4930, I2Cerr : 0}
413: MEMINFO {brkval : 0, freemem : 9568}
413: MISSION_CURRENT {seq : 0}
3084: NAMED_VALUE_FLOAT {time_boot_ms : 399312, name : InputHold, value : 0.0}
413: NAV_CONTROLLER_OUTPUT {nav_roll : 0.710781216621, nav_pitch : 0.192573025823, nav_bearing : 63, target_bearing : 90, wp_dist : 0, alt_error : 1.44385683537, aspd_error : 0.0, xtrack_error : 0.0}
688: PARAM_VALUE {param_id : RC16_DZ, param_value : 0.0, param_type : 4, param_count : 688, param_index : 687}
413: POWER_STATUS {Vcc : 4874, Vservo : 62, flags : 4}
413: RAW_IMU {time_usec : 399152719, xacc : 14, yacc : -20, zacc : -1000, xgyro : 0, ygyro : 1, zgyro : 0, xmag : 306, ymag : -1082, zmag : 754}
399: RC_CHANNELS {time_boot_ms : 399313, chancount : 0, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1500, chan6_raw : 1500, chan7_raw : 0, chan8_raw : 1500, chan9_raw : 1100, chan10_raw : 1100, chan11_raw : 1100, chan12_raw : 0, chan13_raw : 0, chan14_raw : 0, chan15_raw : 0, chan16_raw : 0, chan17_raw : 0, chan18_raw : 0, rssi : 0}
399: RC_CHANNELS_RAW {time_boot_ms : 399313, port : 0, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1500, chan6_raw : 1500, chan7_raw : 0, chan8_raw : 1500, rssi : 0}
413: SCALED_IMU2 {time_boot_ms : 399152, xacc : 22, yacc : -6, zacc : -990, xgyro : 0, ygyro : 2, zgyro : -8, xmag : 0, ymag : 0, zmag : 0}
413: SCALED_PRESSURE {time_boot_ms : 399152, press_abs : 1008.33581543, press_diff : 0.143046870828, temperature : 4226}
38: SENSOR_OFFSETS {mag_ofs_x : 26, mag_ofs_y : -382, mag_ofs_z : 208, mag_declination : 0.0, raw_press : 100832, raw_temp : 4225, gyro_cal_x : -0.0204666107893, gyro_cal_y : 0.0127669684589, gyro_cal_z : 0.020250082016, accel_cal_x : 0.0710098221898, accel_cal_y : -0.135683178902, accel_cal_z : -0.157259613276}
399: SERVO_OUTPUT_RAW {time_usec : 399313037, port : 0, servo1_raw : 1500, servo2_raw : 1500, servo3_raw : 1500, servo4_raw : 1500, servo5_raw : 1500, servo6_raw : 0, servo7_raw : 0, servo8_raw : 0}
4: STATUSTEXT {severity : 6, text : PX4v2 0043003D 3135510E 35333436}
799: SYSTEM_TIME {time_unix_usec : 0, time_boot_ms : 399172}
413: SYS_STATUS {onboard_control_sensors_present : 2161671, onboard_control_sensors_enabled : 2137095, onboard_control_sensors_health : 2161671, load : 315, voltage_battery : 0, current_battery : -1, battery_remaining : -1, drop_rate_comm : 0, errors_comm : 0, errors_count1 : 0, errors_count2 : 0, errors_count3 : 0, errors_count4 : 0}
387: VFR_HUD {airspeed : 0.0, groundspeed : 0.0, heading : 64, throttle : 50, alt : -1.43999993801, climb : -0.019999999553}
386: VIBRATION {time_usec : 399113067, vibration_x : 0.0360655076802, vibration_y : 0.0255210697651, vibration_z : 0.0469667501748, clipping_0 : 0, clipping_1 : 0, clipping_2 : 0}

(Patrick José Pereira) #25

To be able to active guided mode, you need a position fix.

GLOBAL_POSITION_INT {time_boot_ms : 305214, lat : 338103147, lon : -1183938671, alt : -10, relative_alt : -80, vx : 13, vy : 16, vz : -1, hdg : 27496}
GPS_RAW_INT {time_usec : 305214000, fix_type : 6, lat : 338103136, lon : -1183938688, alt : -250, eph : 121, epv : 200, vel : 0, cog : 0, satellites_visible : 10}
LOCAL_POSITION_NED {time_boot_ms : 305314, x : 0.132573872805, y : 0.159943118691, z : 0.357425302267, vx : 0.137562796474, vy : 0.167037427425, vz : 0.0137174353004}

Take a look in this messages, they need to have lat and lon values, check your gps connection and look for GPS_FIX_TYPE > 1. If you solve this, probably you’ll be able to set the guided mode.


Ros + Gazebo SITL : rc/override vs cmd_vel?
(MavNav) #26

I don’t have a GPS module to work with. How can i fix this?


(Patrick José Pereira) #27

You are not able to set speed reference without a position system, the software derivative the position to use it as a velocity reference for the control loop.
Take a look here to check the modes that you can use.


(MavNav) #28

Hmm. Then is it possible to make the rov turn and move directionally with the rc override command? *without gps


(Patrick José Pereira) #29

You can send RC commands using the manual or stabilize mode, but if you want to have velocity or position control you’ll need to create a velocity/position control loop.


(MavNav) #30

I just need it to move and turn as my state machine commands. Would rc commands suffice. Btw im sending them in stabilise mode.

Also how do i deal with the second thruster spinning at max power ?


(Patrick José Pereira) #31

What happen when you use rostopic pub -r 1 /mavros/rc/override mavros_msgs/OverrideRCIn '[1600, 1500, 1500, 1500, 0, 0, 0, 0]' ?


(MavNav) #32

When i connect 1 thruster at a time to the pixhawk (to any of the main outs from 1-4), it runs normally. I can control speed and such. But when i connect 2 thrusters simultaneously, the second one seems to be running at over max speed, making some screeching noises. (I suspect it’s being over-driven).


(Patrick José Pereira) #33

Do you any tool like a logic analyzer or oscilloscope ?
This problem happen when you place the second thruster anywhere in the ROV ?
If you put only one thruster anywhere it work without problem ?
Are you connecting the motors or the ESC ? Are you connecting while the pwm is high ?


(MavNav) #34

Blockquote
Do you any tool like a logic analyzer or oscilloscope ?
This problem happen when you place the second thruster anywhere in the ROV ?
If you put only one thruster anywhere it work without problem ?
Are you connecting the motors or the ESC ? Are you connecting while the pwm is high ?

No,
Yes,
No,
ESC, I connect ESC, then run the code.

Ok something new. It seems to be output 2, 3, and 4 on the pixhawk. I set all the published roll, pitch, yaw, and throttle values to 0 and it still happens on all those pins. Only output 1 works as intended.

This is my code:

#include <ros/ros.h>
#include <std_msgs/String.h> 
#include <stdio.h>
#include <mavros_msgs/OverrideRCIn.h>
int main(int argc, char **argv)
{
	ros::init(argc, argv, "sim_rc");
   	ros::NodeHandle nh;

   	mavros_msgs::OverrideRCIn msg_override;

    ros::Publisher rc_override_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override",10);
    ros::Rate loop_rate(10);

while(ros::ok())
{
	msg_override.channels[0] = 0; //roll
	msg_override.channels[1] = 0; //pitch
	msg_override.channels[2] = 0; //throttle
	msg_override.channels[3] = 0; //yaw
	msg_override.channels[4] = 0; //forward
	msg_override.channels[5] = 0; //lateral
	msg_override.channels[6] = 0;
	msg_override.channels[7] = 0;

	rc_override_pub.publish(msg_override);
    ros::spinOnce();
    loop_rate.sleep();
}
return 0;

}

(Patrick José Pereira) #35

Thank you, I’ll check this and reply asap.


(MavNav) #36

UPDATE: I’ve managed to get it working using Override, successfully controlling each output. turns out, there was a deadzone of 1460 - 1540 which i wasn’t aware of. And that was the “zero” point of control. I’ve yet to test it and will update you accordingly.


(Patrick José Pereira) #37

Good news, please let me know if you have any update.
Yes, the motors have a deadzone, you can check here.


(MavNav) #38

The test for movement went sucessful!
However, there were some things that were off.
The supposed yaw channel actually made the auv move forward, and the forward channel actually makes the auv turn left/right.
Another thing, stabilize only actuates thrusters 1 and 2. How do i do roll stabilization?


(Patrick José Pereira) #39

I’m glad to hear that !

Take a look here to see the channel order, how have you tested the channel actuation ? Can you show the code ?

image

Stabilize only actuators 1 and 2 ? Are you talking about 6 and 5 ?
Have you done all steps after sensor calibration ?


(MavNav) #40

I’m using the frame "simple ROV with 4 thrusters. I also slightly modified the placement of the thrusters.
In this image, Thrusters 1 and 2 are the inner two while Thrusters 3 and 4 are the outer ones.

My stabilise mode doesn’t drive Thrusters 3 and 4 (the outermost 2), which are responsible for roll and depth.

#include <ros/ros.h>
#include <std_msgs/String.h> 
#include <stdio.h>
#include <mavros_msgs/OverrideRCIn.h>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "sim_rc");
   	ros::NodeHandle nh;

   	mavros_msgs::OverrideRCIn msg_override;

    ros::Publisher rc_override_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 1);
    ros::Rate loop_rate(1000); //used to be 10

while(ros::ok())
{
	msg_override.channels[0] = 1500; //roll (NOT USED)
	msg_override.channels[1] = 1500; //pitch (Not USED)
	msg_override.channels[2] = 1300; //throttle (Surface - 1580/ Dive - 1440)
	msg_override.channels[3] = 1500; //yaw (Real Backward - 1440/Forward - 1580)
	msg_override.channels[4] = 1500; //forward (Real Right-1580/Left-1440)
	msg_override.channels[5] = 1500; //lateral (NOT USED)
	msg_override.channels[6] = 0;
	msg_override.channels[7] = 0;

	rc_override_pub.publish(msg_override);
    ros::spinOnce();
    loop_rate.sleep();
}
return 0;
}