From 3efa3cd58ee7e9d0d28ff0283f4df8c02f888633 Mon Sep 17 00:00:00 2001 From: Roland Meertens Date: Thu, 29 Oct 2015 16:44:10 +0100 Subject: [PATCH] fixed remarks by Freek --- conf/airframes/RM/rm_ardrone2.xml | 2 +- conf/messages.xml | 3 +-- conf/modules/uartrotation.xml | 18 +++++--------- conf/telemetry/default_rotorcraft.xml | 1 - .../guidance_opticflow_hover.c | 1 - sw/airborne/modules/optical_flow/px4flow.c | 3 +-- sw/airborne/modules/sensors/stereocam.c | 5 ---- .../modules/uartrotation/uartrotation.c | 24 ++++++------------- .../modules/uartrotation/uartrotation.h | 11 ++++----- 9 files changed, 21 insertions(+), 47 deletions(-) diff --git a/conf/airframes/RM/rm_ardrone2.xml b/conf/airframes/RM/rm_ardrone2.xml index 361a9bc266..b342252369 100644 --- a/conf/airframes/RM/rm_ardrone2.xml +++ b/conf/airframes/RM/rm_ardrone2.xml @@ -35,7 +35,7 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index 9edc17ab9c..4fd11ae1ce 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2092,8 +2092,7 @@ - - + diff --git a/conf/modules/uartrotation.xml b/conf/modules/uartrotation.xml index 83b2aace2b..0c63ac3efe 100644 --- a/conf/modules/uartrotation.xml +++ b/conf/modules/uartrotation.xml @@ -2,33 +2,27 @@ - Writes rotation to the uart bus + Writes rotation to the uart bus. Meant for sending the rotation + to the ODROID board or to the stereo camera boards. + Can be used to improve vision algorithms. + The protocol that is used is the stereocommunication protocol. +
- + - STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z) ap.CFLAGS += -DUSE_$(STEREO_UART) ap.CFLAGS += -DUART_LINK=$(STEREO_UART_LOWER) ap.CFLAGS += -D$(STEREO_UART)_BAUD=$(STEREO_BAUD) - diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index f8de10d0b2..fd59954d1d 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -26,7 +26,6 @@ - diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c index 18cce940bb..bc9e3bc9d3 100644 --- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c +++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c @@ -151,7 +151,6 @@ void guidance_h_module_run(bool_t in_flight) static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, float vel_x, float vel_y, float vel_z) { - printf("Stabilisation before %f %f \n", vel_x, vel_y); /* Check if we are in the correct AP_MODE before setting commands */ if (autopilot_mode != AP_MODE_MODULE) { return; diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c index 4512a85fe6..31ee1f0844 100644 --- a/sw/airborne/modules/optical_flow/px4flow.c +++ b/sw/airborne/modules/optical_flow/px4flow.c @@ -46,9 +46,8 @@ struct mavlink_msg_req req; static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused))) { optical_flow_available = TRUE; - // printf("Stabilisation decoded %d %d height: %f \n", optical_flow.flow_x, optical_flow.flow_y,optical_flow.ground_distance); - // X and Y negated to get to the body of the drone + // Y negated to get to the body of the drone AbiSendMsgVELOCITY_ESTIMATE(PIX4FLOWSENDER_ID, 0, (optical_flow.flow_x/optical_flow.ground_distance), -1.0*(optical_flow.flow_y/optical_flow.ground_distance), diff --git a/sw/airborne/modules/sensors/stereocam.c b/sw/airborne/modules/sensors/stereocam.c index 8a062703a9..b7f5cc4c3f 100644 --- a/sw/airborne/modules/sensors/stereocam.c +++ b/sw/airborne/modules/sensors/stereocam.c @@ -37,12 +37,7 @@ // define coms link for stereocam #define STEREO_PORT (&((UART_LINK).device)) struct link_device *dev = STEREO_PORT; - #define StereoGetch() STEREO_PORT ->get_byte(STEREO_PORT->periph) -#define StereoSend1(c) STEREO_PORT->put_byte(STEREO_PORT->periph, c) -#define StereoUartSend1(c) StereoSend1(c) -#define StereoSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) StereoSend1(_dat[i]); }; -#define StereoUartSetBaudrate(_b) uart_periph_set_baudrate(STEREO_PORT, _b); //typedef struct MsgProperties { // uint16_t positionImageStart; diff --git a/sw/airborne/modules/uartrotation/uartrotation.c b/sw/airborne/modules/uartrotation/uartrotation.c index 2b784b3a21..b06ed27bb4 100644 --- a/sw/airborne/modules/uartrotation/uartrotation.c +++ b/sw/airborne/modules/uartrotation/uartrotation.c @@ -19,47 +19,37 @@ */ /** * @file "modules/readlocationfromodroid/readlocationfromodroid.c" - * @author Roland - * reads from the odroid and sends information of the drone to the odroid using JSON messages + * @author Roland + Kirk + * Sends rotation using the stereoboard protocol over the UART. */ #include "modules/uartrotation/uartrotation.h" #include "subsystems/abi.h" #include -#include #include -#include #include "state.h" #include "subsystems/gps.h" #include "subsystems/ins/ins_int.h" #include "firmwares/rotorcraft/autopilot.h" #include "mcu_periph/uart.h" -#include "subsystems/gps.h" -#include "subsystems/datalink/telemetry.h" #include "subsystems/stereoprotocol.h" #include "navdata.h" -int frameNumberSending=0; +static int frame_number_sending=0; static abi_event odroid_agl_ev; float lastKnownHeight = 0.0; int pleaseResetOdroid = 0; +//uart_periph_set_baudrate(UART_LINK, B115200); -static void write_serial_rot(struct transport_tx *trans, struct link_device *devasdf) { +void write_serial_rot() { struct Int32RMat *ltp_to_body_mat = stateGetNedToBodyRMat_i(); - int32_t lengthArrayInformation = 11*sizeof(int32_t); + static int32_t lengthArrayInformation = 11*sizeof(int32_t); uint8_t ar[lengthArrayInformation]; int32_t *pointer = (int32_t*)ar; for(int indexRot = 0; indexRot < 9; indexRot++){ pointer[indexRot]=ltp_to_body_mat->m[indexRot]; } pointer[9]=(int32_t)(state.alt_agl_f*100); //height above ground level in CM. - pointer[10]=frameNumberSending++; - printf("Whoo sending serial\n"); + pointer[10]=frame_number_sending++; stereoprot_sendArray( &((UART_LINK).device),ar, lengthArrayInformation, 1); - printf("Whoo sending serial2\n"); -} - - -void uart_rotation_init() { - register_periodic_telemetry(DefaultPeriodic, "SERIALRMAT", write_serial_rot); } diff --git a/sw/airborne/modules/uartrotation/uartrotation.h b/sw/airborne/modules/uartrotation/uartrotation.h index 296818e9e7..503594bdba 100644 --- a/sw/airborne/modules/uartrotation/uartrotation.h +++ b/sw/airborne/modules/uartrotation/uartrotation.h @@ -18,15 +18,14 @@ * . */ /** - * @file "modules/readlocationfromodroid/readlocationfromodroid.h" + * @file "modules/uartrotation/uartrotation.h" * @author Roland - * reads from the odroid + * Sends rotation using the stereoboard protocol over the UART. */ -#ifndef READLOCATIONFROMODROID_H -#define READLOCATIONFROMODROID_H +#ifndef UARTROTATION_H +#define UARTROTATION_H #include -extern void odroid_loc_init(void); -extern void odroid_loc_periodic(void); +extern void write_serial_rot(void); #endif