Plane: support DO_SET_HOME MAVLink command

This commit is contained in:
Andrew Tridgell 2015-02-21 10:14:18 +11:00
parent 3a51bac0d0
commit 76f8b9b07c
6 changed files with 31 additions and 6 deletions

View File

@ -672,7 +672,7 @@ static int16_t condition_rate;
static const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location in AHRS
static bool home_is_set;
static enum HomeState home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
static Location prev_WP_loc;
// The plane's current location

View File

@ -1225,6 +1225,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_HOME:
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (packet.param1 == 1) {
init_home();
} else {
if (packet.param5 == 0 && packet.param6 == 0 && packet.param7 == 0) {
// don't allow the 0,0 position
break;
}
Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
ahrs.set_home(new_home_loc);
home_is_set = HOME_SET_NOT_LOCKED;
result = MAV_RESULT_ACCEPTED;
}
break;
}
default:

View File

@ -528,7 +528,7 @@ static void rangefinder_height_update(void)
uint16_t distance_cm = rangefinder.distance_cm();
int16_t max_distance_cm = rangefinder.max_distance_cm();
float height_estimate = 0;
if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set) {
if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set != HOME_UNSET) {
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;

View File

@ -101,7 +101,7 @@ static void init_home()
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
ahrs.set_home(gps.location());
home_is_set = true;
home_is_set = HOME_SET_NOT_LOCKED;
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
@ -125,6 +125,8 @@ static void init_home()
*/
static void update_home()
{
ahrs.set_home(gps.location());
if (home_is_set == HOME_SET_NOT_LOCKED) {
ahrs.set_home(gps.location());
}
barometer.update_calibration();
}

View File

@ -643,7 +643,7 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
init_home();
} else {
ahrs.set_home(cmd.content.location);
home_is_set = true;
home_is_set = HOME_SET_NOT_LOCKED;
}
}

View File

@ -8,7 +8,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd);
static void update_commands(void)
{
if(control_mode == AUTO) {
if (home_is_set) {
if (home_is_set != HOME_UNSET) {
if(mission.state() == AP_Mission::MISSION_RUNNING) {
mission.update();
} else {