Dear Forum,
I try to send a mavlink message out of the 1Hz userhook (ArduCopter321,APM2.6). The code should read in the current position, put some offset to it and send it as a new waypoint with a mavlink-message.
Is there a mistake in my code or isn't it anyway possible to do it this way?
The method mavlink_msg_mission_item_send I copied from control_guided.
In SITL, I go to guided mode and watch a reaction ...
I'm happy for any advice!
here is the UserCode.pde file:
#ifdef USERHOOK_INIT
#include <UARTDriver.h>
/* setup UART at 57600 */
static void setup_uart(AP_HAL::UARTDriver *uart, const char *name)
{
if (uart == NULL) {
return;
}
///begin(baudrate,Rx,Tx)
uart->begin(57600, 256, 256);
}
void userhook_init()
{
/* Setup UartC */
setup_uart(hal.uartC, "uartC");
/* Setup UartA */
setup_uart(hal.uartA, "uartA");
}
#ifdef USERHOOK_SUPERSLOWLOOP
void userhook_SuperSlowLoop()
{
/* write MAVLINK Message every 60 seconds */
#if 1
static unsigned short counter;
counter++;
if (counter == 60)
{
counter = 0;
short _offs_lat = 100;
short _offs_lon = 100;
short _offs_altitude = 1200; /* 12m in centimeters */
int32_t lat = current_loc.lat + _offs_lat;
int32_t lon = current_loc.lng + _offs_lon;
// int32_t alt = _local_gps_altitude + _offs_altitude;
int32_t alt = _offs_altitude; /* assume above ground. (ArduCopter bug.) */
float x = (float) lat / (float) 1e7; /* lat, lon in deg * 10,000,000 */
float y = (float) lon / (float) 1e7;
float z = (float) alt / (float) 100; /* alt in cm */
//mavlink_channel_t upstream_channel = MAVLINK_COMM_1;
//mavlink_channel_t downstream_channel = MAVLINK_COMM_0;
hal.console->printf_P(PSTR("guide x: %f y: %f z: %f\r\n"), x, y, z);
mavlink_msg_mission_item_send(
MAVLINK_COMM_0, /* mavlink_channel_t chan */
255, /* uint8_t target_system */
0, /* uint8_t target_component */
0, /* uint16_t seq: always 0, unknown why. */
MAV_FRAME_GLOBAL, /* uint8_t frame: arducopter uninterpreted */
MAV_CMD_NAV_WAYPOINT, /* uint16_t command: arducopter specific */
2, /* uint8_t current: 2 indicates guided mode waypoint */
0, /* uint8_t autocontinue: always 0 */
0, /* float param1 : hold time in seconds */
5, /* float param2 : acceptance radius in meters */
0, /* float param3 : pass through waypoint */
0, /* float param4 : desired yaw angle at waypoint */
x, /* float x : lat degrees */
y, /* float y : lon degrees */
z /* float z : alt meters */
);
}
#endif
}
Replies
I would suggest looking at the GCS_mavlink.pde as that is where the guided mode mavlink command is processed
https://github.com/diydrones/ardupilot/blob/ArduCopter-3.2.1/ArduCo...
Line 845 is where the command is sent to the autopilot to move
https://github.com/diydrones/ardupilot/blob/ArduCopter-3.2.1/ArduCo...
I'd look at calling those functions directly and avoid the MAVLink fluff.
and look at the source for clues
https://github.com/diydrones/ardupilot/blob/ArduCopter-3.2.1/ArduCo...
Hope that helps :)
It helped, thanks!
We can with
Vector3f nextpos;
nextpos.x = 150*100.00f;
nextpos.y = 150*100.00f;
guided_set_destination(nextpos);
set the next target and it moves in the desired direction :)
Another question (if yout have time), we are trying now for hours to handle the Vector3f xyz0 = pv_location_to_vector(current_loc); method.
After having declared xyz0, like above, we store it into
int32_t x0 = xyz0.x;
int32_t y0 = xyz0.y;
and want to use it in the same userhook, but it doesn't recognize x0 and y0 .... the error message is like it would not have been declared:
error: 'x0' was not declared in this scope
nextpos.x = currentpos.x - x0 + 150*100.00f;
nextpos and currentpos are also type Vector3f. We cannot find the mistake. We tried float, int32_t, double.
The method seems to work like this ... no errors.
Our goal ist to be able to set the next pos relatively to our current...
We got it!
Capital mistake in declaring the variables inside an if-statement ...
Great. next time post a more complete code snippet in http://gist.github.com or similar since it has syntax highlighting and formatting that not supported in comments.
for example https://gist.github.com/billbonney/3a3d35fde2f72fa90060