Skip to content

Commit

Permalink
update various code for errors and warnings or fit reorg
Browse files Browse the repository at this point in the history
  • Loading branch information
gautierhattenberger committed May 18, 2021
1 parent 47bd79c commit b718b55
Show file tree
Hide file tree
Showing 24 changed files with 301 additions and 47 deletions.
33 changes: 33 additions & 0 deletions sw/airborne/arch/linux/subsystems/actuators/actuators_pwm_arch.c
@@ -0,0 +1,33 @@
/*
* Copyright (C) 2010 The Paparazzi Team
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/** @file arch/sim/subsystems/actuators/actuators_pwm_arch.c
* dummy servos handling for linux
*/

#include "subsystems/actuators/actuators_pwm_arch.h"
#include "subsystems/actuators/actuators_pwm.h"

void actuators_pwm_arch_init(void)
{

}

36 changes: 36 additions & 0 deletions sw/airborne/arch/linux/subsystems/actuators/actuators_pwm_arch.h
@@ -0,0 +1,36 @@
/*
* Copyright (C) 2010 The Paparazzi Team
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/** @file arch/sim/subsystems/actuators/actuators_pwm_arch.h
* dummy servos handling for linux
*/

#ifndef ACTUATORS_PWM_ARCH_H
#define ACTUATORS_PWM_ARCH_H

#define SERVOS_TICS_OF_USEC(_v) (_v)

#define ActuatorPwmSet(_i, _v) {}
#define ActuatorsPwmCommit() {}

//extern void actuators_pwm_arch_init(void);

#endif /* ACTUATORS_PWM_ARCH_H */
12 changes: 11 additions & 1 deletion sw/airborne/arch/sim/led_hw.c
@@ -1,10 +1,20 @@
#include "led_hw.h"

#include <stdio.h>
#include <caml/mlvalues.h>
#include <caml/memory.h>
#include <caml/callback.h>

value *leds_closure = 0;
bool led_disable = false;
bool led_disabled = false;

value register_leds_cb(value cb_name)
{
leds_closure = caml_named_value(String_val(cb_name));
return Val_unit;
}

void _led_on(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(1)); }
void _led_off(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(0)); }
void _led_toggle(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(2)); }

21 changes: 10 additions & 11 deletions sw/airborne/arch/sim/led_hw.h
@@ -1,19 +1,18 @@
#ifndef LED_HW_H
#define LED_HW_H

#include <stdio.h>
#include <caml/mlvalues.h>
#include <caml/memory.h>
#include <caml/callback.h>
#include <stdbool.h>

extern value *leds_closure;
extern bool led_disable;
extern bool led_disabled;
extern void _led_on(int i);
extern void _led_off(int i);
extern void _led_toggle(int i);

#define LED_INIT(i) { led_disable = false; }
#define LED_ON(i) { if (leds_closure && !led_disable) callback2(*leds_closure, Val_int(i), Val_int(1)); }
#define LED_OFF(i) { if (leds_closure && !led_disable) callback2(*leds_closure, Val_int(i), Val_int(0)); }
#define LED_TOGGLE(i) { if (leds_closure && !led_disable) callback2(*leds_closure, Val_int(i), Val_int(2)); }
#define LED_DISABLE(i) { LED_OFF(i); led_disable = true; }
#define LED_INIT(i) { led_disabled = false; }
#define LED_ON(i) _led_on(i);
#define LED_OFF(i) _led_off(i);
#define LED_TOGGLE(i) _led_toggle(i);
#define LED_DISABLE(i) { LED_OFF(i); led_disabled = true; }

#define LED_PERIODIC() {}

Expand Down
27 changes: 27 additions & 0 deletions sw/airborne/arch/sim/mcu_periph/gpio_arch.c
@@ -0,0 +1,27 @@
/*
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/**
* @file arch/sim/mcu_periph/gpio_arch.c
*
* Dummy file for GPIO
*/


6 changes: 0 additions & 6 deletions sw/airborne/arch/sim/sim_ap.c
Expand Up @@ -151,9 +151,3 @@ value set_datalink_message(value s)
return Val_unit;
}

/** Required by electrical */
void adc_buf_channel(void *a __attribute__((unused)),
void *b __attribute__((unused)),
void *c __attribute__((unused)))
{
}
3 changes: 3 additions & 0 deletions sw/airborne/boards/ardrone/navdata.c
Expand Up @@ -132,6 +132,8 @@ ssize_t full_read(int fd, uint8_t *buf, size_t count)

static void send_navdata(struct transport_tx *trans, struct link_device *dev)
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
&navdata.measure.taille,
&navdata.measure.nu_trame,
Expand Down Expand Up @@ -162,6 +164,7 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev)
&navdata.measure.mz,
&navdata.measure.chksum,
&navdata.checksum_errors);
#pragma GCC diagnostic pop
}
#endif

Expand Down
3 changes: 3 additions & 0 deletions sw/airborne/inter_mcu.h
Expand Up @@ -46,6 +46,9 @@
#include "subsystems/electrical.h"
#include "firmwares/fixedwing/main_fbw.h"

/** FIXME dummy definition for compat with rotorcraft **/
static inline void intermcu_init(void) {}
static inline void intermcu_periodic(void) {}

/** Data structure shared by fbw and ap processes */
struct fbw_state {
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/math/pprz_trig_int.c
Expand Up @@ -854,8 +854,14 @@ inline int16_t pprz_trig_int_f(int32_t angle)
#endif
}

#else

// dummy init
int pprz_trig_int_init(void) { return 0; }

#endif // PPRZ_TRIG_INT_COMPR_FLASH


int32_t pprz_itrig_sin(int32_t angle)
{
#if defined(PPRZ_TRIG_INT_USE_FLOAT)
Expand Down
6 changes: 4 additions & 2 deletions sw/airborne/math/pprz_trig_int.h
Expand Up @@ -53,7 +53,7 @@ extern "C" {
#endif

#define TRIG_INT_SIZE 6434
#define TRIG_INT_VAL_MAX 14
#define TRIG_INT_VAL_MAX 14
#define TREE_SIZE_4 (TRIG_INT_VAL_MAX - 4)
#define TREE_SIZE_8 (TRIG_INT_VAL_MAX - 8)
#define TREE_SIZE_12 (TRIG_INT_VAL_MAX - 12)
Expand All @@ -78,9 +78,11 @@ extern int32_t pprz_itrig_cos(int32_t angle);
extern int32_t int32_atan2(int32_t y, int32_t x);
extern int32_t int32_atan2_2(int32_t y, int32_t x);

// init compressed table if needed
extern int pprz_trig_int_init(void);

#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
uint8_t get_nibble(uint16_t pos);
int pprz_trig_int_init(void);

#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
void table_encode_4(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab);
Expand Down
8 changes: 5 additions & 3 deletions sw/airborne/modules/datalink/mavlink.c
Expand Up @@ -309,7 +309,7 @@ void mavlink_common_message_handler(const mavlink_message_t *msg)
}
break;

#ifndef AP
#ifdef ROTORCRAFT_FIRMWARE
/* only for rotorcraft */
case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd;
Expand Down Expand Up @@ -438,7 +438,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
{
uint8_t mav_state = MAV_STATE_CALIBRATING;
uint8_t mav_mode = 0;
#ifdef AP
#if defined(FIXEDWING_FIRMWARE)
uint8_t mav_type = MAV_TYPE_FIXED_WING;
switch (autopilot_get_mode()) {
case AP_MODE_MANUAL:
Expand All @@ -456,7 +456,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
default:
break;
}
#else
#elif defined(ROTORCRAFT_FIRMWARE)
uint8_t mav_type = MAV_TYPE_QUADROTOR;
switch (autopilot_get_mode()) {
case AP_MODE_HOME:
Expand Down Expand Up @@ -486,6 +486,8 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
default:
break;
}
#else
#error "mavlink datalink: unsupported firmware"
#endif
if (stateIsAttitudeValid()) {
if (autopilot_throttle_killed()) {
Expand Down
6 changes: 4 additions & 2 deletions sw/airborne/modules/datalink/missionlib/mission_manager.c
Expand Up @@ -38,10 +38,12 @@
#pragma GCC diagnostic pop

// for waypoints, include correct header until we have unified API
#ifdef AP
#if defined(FIXEDWING_FIRMWARE)
#include "subsystems/navigation/common_nav.h"
#else
#elif defined(ROTORCRAFT_FIRMWARE)
#include "firmwares/rotorcraft/navigation.h"
#else
#error "Mission manager: unsupported firmware"
#endif
#include "generated/flight_plan.h"

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/datalink/missionlib/waypoints.c
Expand Up @@ -48,7 +48,7 @@
static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < NB_WAYPOINT) { // Due to indexing
#ifdef AP
#ifdef FIXEDWING_FIRMWARE
/* for fixedwing firmware send as LOCAL_ENU for now */
mavlink_msg_mission_item_send(MAVLINK_COMM_0,
sysid,
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/digital_cam/dc.c
Expand Up @@ -35,7 +35,7 @@
#include "dc.h"

// for waypoints, include correct header until we have unified API
#ifdef AP
#ifdef FIXEDWING_FIRMWARE
#include "subsystems/navigation/common_nav.h"
#else
#include "firmwares/rotorcraft/navigation.h"
Expand Down

0 comments on commit b718b55

Please sign in to comment.