mirror of https://github.com/ArduPilot/ardupilot
82 lines
2.4 KiB
C++
82 lines
2.4 KiB
C++
|
/*
|
||
|
|
* This file is free software: you can redistribute it and/or modify it
|
||
|
|
* under the terms of the GNU General Public License as published by the
|
||
|
|
* Free Software Foundation, either version 3 of the License, or
|
||
|
|
* (at your option) any later version.
|
||
|
|
*
|
||
|
|
* This file is distributed in the hope that it will be useful, but
|
||
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||
|
|
* See the GNU General Public License for more details.
|
||
|
|
*
|
||
|
|
* You should have received a copy of the GNU General Public License along
|
||
|
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
// This is very minimal example of using libAP_JSON to send simulator data to ArduPilot SITL
|
||
|
|
|
||
|
|
#include <time.h>
|
||
|
|
#include <chrono>
|
||
|
|
#include <stdlib.h>
|
||
|
|
|
||
|
|
#include "libAP_JSON.cpp"
|
||
|
|
|
||
|
|
uint16_t servo_out[16];
|
||
|
|
|
||
|
|
uint64_t micros() {
|
||
|
|
uint64_t us = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::
|
||
|
|
now().time_since_epoch()).count();
|
||
|
|
return us;
|
||
|
|
}
|
||
|
|
|
||
|
|
int main() {
|
||
|
|
// init the ArduPilot connection
|
||
|
|
libAP_JSON ap;
|
||
|
|
if (ap.InitSockets("127.0.0.1", 9002))
|
||
|
|
{
|
||
|
|
std::cout << "started socket" << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// send and receive data from AP
|
||
|
|
while (true)
|
||
|
|
{
|
||
|
|
double timestamp = (double) micros() / 1000000.0;
|
||
|
|
|
||
|
|
if (ap.ReceiveServoPacket(servo_out))
|
||
|
|
{
|
||
|
|
#if DEBUG_ENABLED
|
||
|
|
std::cout << "servo_out PWM: [";
|
||
|
|
for (int i = 0; i < MAX_SERVO_CHANNELS - 1; ++i)
|
||
|
|
{
|
||
|
|
std::cout << servo_out[i] << ", ";
|
||
|
|
}
|
||
|
|
std::cout << servo_out[MAX_SERVO_CHANNELS - 1] << "]" << std::endl;
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!ap.ap_online) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// example rangefinder data
|
||
|
|
double rangefinder_example[] = {1, 2, 3, 4, 5, 6};
|
||
|
|
|
||
|
|
// set the optionals
|
||
|
|
ap.setAirspeed(1);
|
||
|
|
ap.setWindvane(1 , 1);
|
||
|
|
ap.setRangefinder(rangefinder_example, 6);
|
||
|
|
|
||
|
|
// send with the required
|
||
|
|
ap.SendState(timestamp,
|
||
|
|
0, 0, 0, // gyro
|
||
|
|
0, 0, -9.81, // accel
|
||
|
|
0, 0, 0, // position
|
||
|
|
0, 0, 0, // attitude
|
||
|
|
0, 0, 0); // velocity
|
||
|
|
|
||
|
|
usleep(1000); // run this example at about 1000 Hz. Realistically it is about 800 Hz.
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|