I am working on a project in which I am controlling a quadcopter with an android phone. I have noticed that the using the DroneListener to listen for ATTITUDE_UPDATED messages is not updating the yaw nearly as fast enough as I need it to. I would like to get the quadcopter’s yaw updated frequently, in (more or less) real time, such as how the Mission Planner application does when you plug your quadcopter in via usb.
I have tried using msg_request_data_stream (that is essentially what Mission Planner uses, but in C#) to request various data streams from the drone, but it has not worked. My assumption is that such the response to this request (i.e., the stream) would be received by any registered MavlinkObservers, however the custom MavlinkObserver that I have added to my drone has not had its onMavlinkMessageReceived(…) method called a single time. It has not even received heartbeat messages, which should (based on my understanding) be coming reqularly and frequently.
Is there any help that you could offer? Is there any better way to get the quadcopter’s yaw than waiting for the DroneListener to be notified of events? I know that it should be possible with Mavlink (MissionPlanner does it), but I haven’t been having any success with Mavlink messages or with a MavlinkObserver.
Here is my code for requesting a data stream:
msg_request_data_stream message = new msg_request_data_stream();
//not sure how doubles cast to byte, so I cast it to an int first (the value is
//always going to be an int anyway, getValue just returns doubles for parameters by default)
message.target_system = (byte)((int)params.getParameter("SYSID_THISMAV").getValue());
message.req_message_rate = (short)1;
message.req_stream_id = (byte)0;
message.start_stop = (byte)1;
//wrap the message and send it!
ExperimentalApi.sendMavlinkMessage(this, new MavlinkMessageWrapper(message));
Related
I am trying to control a robot using a template-based controller class written in c++. Essentially I have a UDP connection setup with the robot to receive the state of the robot and send new torque commands to the robot. I receive new observations at a higher frequency (say 2000Hz) and my controller takes about 1ms (1000Hz) to calculate new torque commands to send to the robot. The problem I am facing is that I don't want my main code to wait to send the old torque commands while my controller is still calculating new commands to send. From what I understand I can use Ubuntu with RT-Linux kernel, multi-thread the code so that my getTorques() method runs in a different thread, set priorities for the process, and use mutexes and locks to avoid data race between the 2 threads, but I was hoping to learn what the best strategies to write hard-realtime code for such a problem are.
// main.cpp
#include "CONTROLLER.h"
#include "llapi.h"
void main{
...
CONTROLLERclass obj;
...
double new_observation;
double u;
...
while(communicating){
get_newObs(new_observation); // Get new state of the robot (2000Hz)
obj.getTorques(new_observation, u); // Takes about 1ms to calculate new torques
send_newCommands(u); // Send the new torque commands to the robot
}
...
}
Thanks in advance!
Okay, so first of all, it sounds to me like you need to deal with the fact that you receive input at 2 KHz, but can only compute results at about 1 KHz.
Based on that, you're apparently going to have to discard roughly half the inputs, or else somehow (in a way that makes sense for your application) quickly combine the inputs that have arrived since the last time you processed the inputs.
But as the code is structured right now, you're going to fetch and process older and older inputs, so even though you're producing outputs at ~1 KHz, those outputs are constantly being based on older and older data.
For the moment, let's assume you want to receive inputs as fast as you can, and when you're ready to do so, you process the most recent input you've received, produce an output based on that input, and repeat.
In that case, you'd probably end up with something on this general order (using C++ threads and atomics for the moment):
std::atomic<double> new_observation;
std::thread receiver = [&] {
double d;
get_newObs(d);
new_observation = d;
};
std::thread sender = [&] {
auto input = new_observation;
auto u = get_torques(input);
send_newCommands(u);
};
I've assumed that you'll always receive input faster than you can consume it, so the processing thread can always process whatever input is waiting, without receiving anything to indicate that the input has been updated since it was last processed. If that's wrong, things get a little more complex, but I'm not going to try to deal with that right now, since it sounds like it's unnecessary.
As far as the code itself goes, the only thing that may not be obvious is that instead of passing a reference to new_input to either of the existing functions, I've read new_input into variable local to the thread, then passed a reference to that.
I am writing a ParaView version 5.1.2 plugin in C++ to visualize point cloud data produced by a LiDAR sensor. I noticed that Velodyne has an open source ParaView custom application to visualize their LiDAR data called Veloview. I tweaked some of their code to start but I am stuck now.
So far I wrote a reader that takes a pcap file and renders a point cloud that can be played back frame by frame. I also wrote a ParaView source that listens on a port and captures udp packets and after they are captured uses the reader to split them into frames and visualize the PC.
Now I would like to take live udp packets and render the point cloud in real time as each frame is completed.
I am having trouble accomplishing this because of the ParaView plugin structure. Currently, my reader displays a frame when the method RequestData is called. My method looks something like this.
int RequestData(vtkInformation *request, vtkInformationVector **inputVector, vtkInformationVector *outputVector){
vtkPolyData* output = vtkPolyData::GetData(outputVector);
vtkInformation* info = outputVector->GetInformationObject(0);
int timestep = 0;
if (info->Has(vtkStreamingDemandDrivenPipeline::UPDATE_TIME_STEP()))
{
double timeRequest = info->Get(vtkStreamingDemandDrivenPipeline::UPDATE_TIME_STEP());
int length = info->Length(vtkStreamingDemandDrivenPipeline::TIME_STEPS());
timestep = static_cast<int>(floor(timeRequest + 0.5));
}
this->Open();
// GetFrame returns a vtkSmartPointer<vtkPolyData> that is the frame
output->ShallowCopy(this->GetFrame(timestep));
this->Close();
return 1;
}
The RequestData method is called every time the timestep is updated in the ParaView gui. Then the frame from that timestep is copied into the outputVector.
I am not sure how to implement this with live data because in that circumstance the RequestData method is not called because no timesteps are requested. I saw there is a way to keep RequestData executing by using CONTINUE_EXECUTING() in this way.
request->Set(vtkStreamingDemandDrivenPipeline::CONTINUE_EXECUTING(), 1);
But I do not know if that is supposed to be used to visualize live data.
For now I am interested in simply reading live packets and throwing them away as soon as their frame is rendered. Does anyone know how I can achieve this?
In the code of VeloView (which basically is a bundled ParaView+LidarPlugin), the timesteps of ParaView is changed by the main code, not the Lidar Plugin.
We advice you to start from VeloView code, which is much closer to your goal.
If you really want to start from scratch within ParaView, you need to increment this requested timestep yourself.
Newest version of VeloView (unreleased) uses the same mechanism as ParaView “LiveSource” plugin (available in 5.6+), where the plugin tells ParaView to set a QtTimer that will automatically increment the available and requested timesteps.
request->Set(vtkStreamingDemandDrivenPipeline::CONTINUE_EXECUTING(), 1); relates to another mechanism that will run request Data multiple time, but won’t take care of updating the requested timestep.
Best,
Bastien Jacquet
VeloView project leader
I am new to gRPC framework, and I have created a sample client-server on my PC (referring to this).
In my client-server application I have implemented a simple RPC
service NameStudent {
rpc GetRoll(RollNo) returns (Details) {}
}
The client sends a RollNo and receives his/her details which are name, age, gender, parent name, and roll no.
message RollNo{
int32 roll = 1;
}
message Details {
string name = 1;
string gender = 2;
int32 age = 3;
string parent = 4;
RollNo rollid = 5;
}
The actual server and client codes are adaptation of the sample code explained here
Now my server is able to listen to "0.0.0.0:50051(address:port)" and client is able to send the roll no on "localhost:50051" and receive the details.
I want to see the actual binary data that is transferred between client and server. i have tried using Wireshark, but I don't understand what I am seeing here.
Here is the screenshot of wireshark capture
And here are the details of highlighted entry from above screenshot.
Need help in understanding wireshark here, Or any other way that can be used to see the binary data.
Wireshark uses the port to determine how to decode the communication, and it doesn't know any protocol associated with 50051. So you need to configure it to treat this as HTTP.
Right click on a row and select "Decode As..." in the context menu.
Then set "Current" to "HTTP" or "HTTP2" (HTTP will generally auto-detect HTTP2) and hit "OK".
Then the HTTP/2 frames should be decoded. And if using a recent version of Wireshark, you may also see the gRPC frames decoded.
The whole idea of grpc is to HIDE that. Let's say we ignore that and you know what you're doing.
Look at https://en.wikipedia.org/wiki/Protocol_Buffers. gRPC uses Protocol Buffers for it's data representation. You might get a hint at the data you're seeing.
Two good starting points for a reverse engineer exercise are:
Start simple: compile a program that sends an integer. Understand it. Sniff it. Then compile a program that sends a string. Try several values. Once you understand it, pass to tacke the problem of understanding how's google sending your structure.
Use known data and do small variations: knowing what 505249... means is easier if you start knowing the data you're sending (as an example, send "Hello world" string; then change it to "Hella world"; see what changes on the coded sniff; also check that sending several times the same data produces the same sniffed output). Apply prior point: start simple, first empty string, then " ", then "a", then "b", etc. and then pass to complex and larger strings. Don't be affraid to start simple.
The following code is inside a thread and reads input data coming over usb. Approximately every 80 readings it misses one of the packets coming from an stm32 board. The board is programmed to send data packets to the android tablet every one second.
// Non Working Code
while(running){
int resp = bulktransfer(mInEp,mBuf,mBuf.lenght,1000);
if(resp>0){
dispatchMessage(mBuf);
}else if(resp<0)
showsBufferEmptyMessage();
}
I was looking the Missile Launcher example in android an other libraries on the internet and they put a delay of 50ms between each poll. Doing this it solves the missing package problem.
//Working code
while(running){
int resp = bulktransfer(mInEp,mBuf,mBuf.lenght,1000);
if(resp>0){
dispatchMessage(mBuf);
}else if(resp<0)
showsBufferEmptyMessage();
try{
Thread.sleep(50);
}catch(Exception e){}
}
Does anyone knows the reason why the delay works. Most of the libraries on github has this delay an as I mention before the google example too.
I am putting down my results regarding this problem. After all seems that the UsbConnection.bulkTransfer(...) method has some bug when called continuously. The solution was to use the asynchronous API, UsbRequest class. Using this method I could read from the input endpoint without delay and no data was lost during the whole stress test. So the direction to take is asynchronous UsbRequest instead of synchronously bulktransfer.
I'm currently building a robot which has some sensors attached to it. The control unit on the robot is an ARM Cortex-M3, all sensors are attached to it and it is connected via Ethernet to the "ground station".
Now I want to read and write settings on the robot via the ground station. Therefore I thought about implementing a "virtual register" on the robot that can be manipulated by the ground station.
It could be made up of structs and look like this:
// accelerometer register
struct accel_reg {
// accelerations
int32_t accelX;
int32_t accelY;
int32_t accelZ;
};
// infrared distance sensor register
struct ir_reg {
uint16_t dist; // distance
};
// robot's register table
struct {
uint8_t status; // current state
uint32_t faultFlags; // some fault flags
accel_reg accelerometer; // accelerometer register
ir_reg ir_sensors[4]; // 4 IR sensors connected
} robot;
// usage example:
robot.accelerometer.accelX = -981;
robot.ir_sensors[1].dist = 1024;
On the robot the registers will be constantly filled with new values and configuration settings are set by the ground station and applied by the robot.
The ground station and the robot will be written in C++ so they both can use the same struct datatype.
The question I have now is how to encapsulate the read/write operations in a protocol without writing tons of meta data?
Let's say I want to read the register robot.ir_sensors[2].dist. How would I address this register in my protocol?
I already thought about transmitting a relative offset in bytes (i.e the relative position in memory inside the struct) but I think memory alignment and padding may cause problems, especially because the ground station runs on a x86_64 architecture and the robot runs on a 32-bit ARM processor.
Thanks for any hints! :)
I'm also going to suggest Google Protocol Buffers.
In the simplest case, you could implement one message RobotState like this:
message RobotState {
optional int32_t status = 1;
optional int32_t distance = 2;
optional int32_t accelX = 3;
...
}
Then when the robot receives the message, it will take new values from any optional field that is present. It will then reply with a message containing the current value of all fields.
This way it is quite easy to implement field update using the "merge message" functionality of most protobuf implementations. Also you can keep it very simple at start because you only have one message type, but if you need to expand later you can add submessages.
It is true that protobuf does not support int8_t or int16_t. Just use int32_t instead.
I think the Google protocol buffers are an excellent session/presentation layer tool to use. Actually, Google protocol buffers do not support the syntax I am thinking of. So, I will change this part of my answer to recommend XSD by Code Synthesis. Although it is primarily used with XML, it supports different presentation layers such as XDR and may be more efficient than protocol buffers with large amounts of optional data. The generate code is also very nice to work with. XSD is free to use with OpenSource software and even commercial use with limited message structures.
I don't believe you want to read/write register sets at random. You can prefix a message with an enum that denotes a message such as, IR update, distance, accel, etc. These are register groups. Then the robot responds with the register set. All the registers you've given so far are sensors. The write ones must be motor control?
You want to think about what control you want to perform and the type of telemetry you would like to receive. Then come up with a message structure and bundle the information together. You could use sequence diagrams, and remote procedure API's like SOA/SOAP, RPC, REST, etc. I don't mean these RPC frameworks directly, but the concepts such as request/response and perhaps message that are just sent periodically (telemetry) without specific requests. So there would be a telemetry request from the ground station with some sort of interval and then the robot would respond periodically with unsolicited data. You always need a message id (enum above), unless your protocol is going to be stateful, which I would discourage for robustness reasons.
You haven't described how the control system might work or if you wish to do this remotely. Describing that may lead to more ideas on the protocol. I believe we are talking about layers 5,6,7 of OSI. Have fun.