Skip to content

Set home position required. #649

Open
@hamishwillee

Description

@hamishwillee

#646 adds a home position listener that sets our home position attribute when the appropriate message is received (this will be sent by autopilot whenever the home position is changed). The same dk-python attribute is also set when waypoint_0 is read (the "legacy" home position location).

We need to make a few additional changes:

  • Add attribute listener for home message (done in adds notification whenever home location is changed #650)
  • Create a setter to set the value rather than using attribute (as discussed in Create setter functions rather than setting attributes directly #566).
    • The setter currently uses MAV_CMD_DO_SET_HOME rather than SET_HOME_POSITION. The former adds ability to set the home position as current position (which we don't use) but seems to be very slightly lossy in "round trip" - ie what you set using the MAV_CMD is not exactly what you get back from the HOME_POSITION.

    • Propose a setter than uses both methods, and can set the value to home position. Something like:

      def set_home_position(self,location=None):
        if not isinstance(pos, LocationGlobal):
        raise Exception('Excepting home_location to be set to a LocationGlobal.')
      
        # Set cached home location.
        self._home_location = copy.copy(pos)
      
        if location:
            msg = this.message_factory.set_home_position_encode(
                0,       # System ID maybe
                location.lat * 1.0e7,       # lat - int32_t Latitude (WGS84), in degrees \* 1E7
                location.lon * 1.0e7,       # lon - int32_t Longitude (WGS84), in degrees \* 1E7
                location.alt * 1000,       # alt - int32_t  Altitude (AMSL), in meters \* 1000 (positive for up)
                0, 0, 0, [1,1,1,1], 0, 0, 0 #Remaining parameters not supported by ardupilot.
                )    
            # send command to vehicle
            this.send_mavlink(msg)
        else:
            #use current location as home location
            self.send_mavlink(self.message_factory.command_long_encode(
            0, 0, mavutil.mavlink.MAV_CMD_DO_SET_HOME, 0, 1, 0, 0, 0, 0, 0, 0))
      
  • Add code to request the home position on DK connection (otherwise we won't receive it if the home position has already been set).
  • Add code to disable setting/getting the value from waypoint_0 if the message is supported. The best way to do this is to read the capabilities - but not yet supported: Need capabilities for HOME_POSITION support (all messages) ArduPilot/ardupilot#4503

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions