diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:48:09 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:48:09 +0100 |
| commit | b52402dbe2d3447b25a46070bdd771c12fd4c55a (patch) | |
| tree | 825b9a3349169ce933906685f05ada037fd29570 /apps/mavlink/missionlib.c | |
| parent | fdf1c712b163deace14dfd8c74a6b1afac6262fb (diff) | |
| download | px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.tar.gz px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.tar.bz2 px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.zip | |
Fixed code style for mavlink app
Diffstat (limited to 'apps/mavlink/missionlib.c')
| -rw-r--r-- | apps/mavlink/missionlib.c | 12 |
1 files changed, 11 insertions, 1 deletions
diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c index 50282a9e3..8064febc4 100644 --- a/apps/mavlink/missionlib.c +++ b/apps/mavlink/missionlib.c @@ -106,6 +106,7 @@ mavlink_missionlib_send_gcs_string(const char *string) mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); return mavlink_missionlib_send_message(&msg); + } else { return 1; } @@ -144,12 +145,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -160,12 +164,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { @@ -175,15 +182,18 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } + sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } - + mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); |