mavlink message out of userhook

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
}

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Developer

    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...

This reply was deleted.

Activity

DIY Robocars via Twitter
RT @chr1sa: Donkeycar 4.4 released with tons of new features, including path learning (useful with GPS outdoors), better Web and Lidar supp…
yesterday
DIY Robocars via Twitter
RT @NXP: We are already biting our nails in anticipation of the #NXPCupEMEA challenge! 😉 Did you know there are great cash prizes to be won…
Friday
DIY Robocars via Twitter
RT @gclue_akira: レースまであと3日。今回のコースは激ムズかも。あと一歩 #jetracer https://t.co/GKcEjImQ3t
Friday
DIY Robocars via Twitter
UC Berkeley's DIY robocar program https://roar.berkeley.edu/
Friday
DIY Robocars via Twitter
RT @chr1sa: The next @DIYRobocars autonomous car race at @circuitlaunch will be on Sat, Dec 10. Thrills, spills and a Brazilian BBQ. Fun…
Friday
DIY Robocars via Twitter
RT @arthiak_tc: Donkey car platform ... Still training uses behavioral cloning #TCXpo #diyrobocar @OttawaAVGroup https://t.co/PHBYwlFlnE
Nov 20
DIY Robocars via Twitter
RT @emurmur77: Points for style. @donkeycar racing in @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video. https://t.co/Y2hMyj1…
Nov 20
DIY Robocars via Twitter
RT @SmallpixelCar: Going to @diyrobocars race at @UCSDJacobs https://t.co/Rrf9vDJ8TJ
Nov 8
DIY Robocars via Twitter
RT @SmallpixelCar: Race @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video. https://t.co/kK686Hb9Ej
Nov 8
DIY Robocars via Twitter
RT @PiWarsRobotics: Presenting: the Hacky Racers Robotic Racing Series in collaboration with #PiWars. Find out more and register your inter…
Oct 23
DIY Robocars via Twitter
RT @Hacky_Racers: There will be three classes at this event: A4, A2, and Hacky Racer! A4 and A2 are based around UK paper sizing and existi…
Oct 23
DIY Robocars via Twitter
Oct 23
DIY Robocars via Twitter
Oct 19
DIY Robocars via Twitter
Oct 18
DIY Robocars via Twitter
RT @NeaveEng: Calling all UK based folks interested in @diyrobocars, @f1tenth, @donkey_car, and similar robot racing competitions! @hacky_r…
Oct 13
DIY Robocars via Twitter
RT @araffin2: 🏎️ After hours of video editing, I'm happy to share a best of my Twitch videos on learning to race with RL. 🏎️ Each part is…
Oct 13
More…